serialRobotURDF.py

You can view and download this file on Github: serialRobotURDF.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Example of a serial robot with URDF file
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2024-11-07
  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.utilities import * #includes itemInterface and rigidBodyUtilities
 16import exudyn.graphics as graphics #only import if it does not conflict
 17#from exudyn.rigidBodyUtilities import *
 18from exudyn.robotics import *
 19from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
 20from exudyn.robotics.utilities import GetRoboticsToolboxInternalModel, LoadURDFrobot, GetURDFrobotData
 21
 22import numpy as np
 23from numpy import linalg as LA
 24from math import pi
 25import sys
 26
 27#%%+++++++++++++++++++
 28SC = exu.SystemContainer()
 29mbs = SC.AddSystem()
 30
 31mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard(point=[1.5,2,0], size=6)])
 32
 33#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 34#%%+++++++++++++++++++++++++++++++
 35robotModels = ['UR3', 'UR5', 'UR10', 'Puma560', 'LBR', 'Panda', 'vx300', 'wx250', 'px150']
 36robotModels = ['UR5'] #only one robot
 37onlyShowModel = False #True: only graphics, no simulation
 38listKinematicTree = []
 39listTrajectory = []
 40saveHDF5 = False        #use this to save robot data, allowing to load directly from there
 41loadFromHDF5 = False#faster load; does not require roboticstoolbox, pymeshlab, etc.
 42
 43for modelCnt, modelName in enumerate(robotModels):
 44    if not loadFromHDF5:
 45        urdfRobotDict = GetRoboticsToolboxInternalModel(modelName, ignoreURDFerrors=False)
 46
 47        urdf = urdfRobotDict['urdf']
 48        robot = urdfRobotDict['robot']
 49        linkColorList = None
 50        if modelCnt < 6: #robots have no nice colors
 51            linkColorList = [graphics.color.grey, graphics.color.lightgrey]*10
 52        robotDataDict = GetURDFrobotData(robot, urdf,
 53                                         returnStaticGraphicsList=onlyShowModel,
 54                                         linkColorList=linkColorList)
 55
 56        if saveHDF5:
 57            from exudyn.advancedUtilities import SaveDictToHDF5
 58            SaveDictToHDF5('solution/robot'+modelName+'.h5', robotDataDict)
 59    else:
 60        from exudyn.advancedUtilities import LoadDictFromHDF5
 61        robotDataDict = LoadDictFromHDF5('solution/robot'+modelName+'.h5')
 62
 63    #%%+++++++++++++++++
 64    #draw in roboticstoolbox:
 65    #robot.plot(robot.qr) #, backend='swift')
 66    offsetPosition = np.array([modelCnt%3,1.3*int(modelCnt/3),0])
 67    #if modelName=='LBR': offsetPosition[0]-=0.8 #robot not placed at [0,0,0]
 68
 69    #static view of robot in Exudyn with staticGraphicsList
 70    if onlyShowModel:
 71        if modelCnt == 0: mbs.CreateMassPoint(physicsMass=1) #to avoid zero-DOF system
 72        mbs.CreateGround(referencePosition=offsetPosition,
 73                         graphicsDataList=robotDataDict['staticGraphicsList'])
 74        # continue
 75
 76
 77    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 78    linkList = robotDataDict['linkList']
 79    numberOfJoints = robotDataDict['numberOfJoints']
 80
 81    if len(linkList) != numberOfJoints:
 82        #if this does not work for robots, probably some weird configuration or branches/multiple arms
 83        raise ValueError('robot creation: inconsistent data!')
 84
 85
 86    graphicsBaseList = robotDataDict['graphicsBaseList']
 87    graphicsToolList = robotDataDict['graphicsToolList']
 88
 89    #some control parameters, per joint: (have to be replaced by real values)
 90    Pcontrol = np.array([1e5, 1e5, 1e5, 1e3, 1e3, 1e3]+[1e3]*(numberOfJoints-6))
 91    Dcontrol = np.array([1e3, 1e3, 1e2, 1e1, 1e1, 1e1]+[1e1]*(numberOfJoints-6))
 92    if (modelName.startswith('vx')
 93        or modelName.startswith('wx')
 94        or modelName.startswith('px')):
 95        Pcontrol[-3:] *= 1e3 #we need stiffer controller!
 96        Dcontrol[-3:] *= 1e3
 97
 98    #changed to new robot structure July 2021:
 99    mbsRobot = Robot(gravity=[0,0,9.81],
100                  base = RobotBase(HT = HTtranslate(offsetPosition),
101                                   visualization=VRobotBase(graphicsData=graphicsBaseList)),
102                  tool = RobotTool(HT=HTtranslate([0,0,0]), visualization=VRobotTool(graphicsData=graphicsToolList)),
103                  referenceConfiguration = []) #referenceConfiguration created with 0s automatically
104
105
106    #++++++++++++++++++++++++++
107    #according numbering of links (0,1,2,...)
108    linkNumbers={'None':-1}
109    linkNumbers['base_link']=-1
110    for cnt, link in enumerate(linkList):
111        linkNumbers[link['name']] = cnt
112
113    #++++++++++++++++++++++++++
114    print('*** load model '+modelName+', nJoints=',numberOfJoints)
115    for cnt, link in enumerate(linkList):
116        jointType = None
117        HT = link['preHT']
118        #if cnt <= 3: print('pos =',HT2translation(HT))
119
120        if len(link['graphicsDataList']) == 0: #no graphics loaded (no urdf, no pymeshlab)
121            visualization=VRobotLink(linkColor=graphics.colorList[cnt])
122        else:
123            jointWidth=0.1 #graphical parameters
124            jointRadius=0.06
125            visualization=VRobotLink(graphicsData=link['graphicsDataList'],
126                                     showMBSjoint=False,
127                                     jointWidth=0.12,jointRadius=0.01)
128        mass = link['mass']
129        inertia = link['inertiaCOM']
130        if mass == 0: #some robots have no mass/inertia => use some values to make it run
131            if cnt == 0 and mass == 0:
132                print('robot has mass=0: assuming some values instead!')
133            mass = 5/(1+2*cnt)
134            inertia = InertiaSphere(mass, 0.25/(1+2*cnt)).InertiaCOM()
135        # else:
136        #     print('mass',cnt,'=',mass)
137        mbsRobot.AddLink(RobotLink(mass=mass,
138                                   parent = linkNumbers[link['parentName']],
139                                   COM=link['com'],
140                                   inertia=inertia,
141                                   preHT=link['preHT'],
142                                   jointType=link['jointType'],
143                                   PDcontrol=(Pcontrol[cnt], Dcontrol[cnt]),
144                                   visualization=visualization
145                                   ))
146
147    #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
148    #configurations and trajectory
149    q0 = np.zeros(numberOfJoints) #zero angle configuration
150    q1 = np.zeros(numberOfJoints)
151    q2 = np.zeros(numberOfJoints)
152    q3 = np.zeros(numberOfJoints)
153    q0 = robotDataDict['staticJointValues']
154
155    #this set also works with load control:
156    q1b = [0.5*pi,0,       -0.25*pi,        0,0,0] #zero angle configuration
157    q2b = [0,     pi/8,     pi*0.5, 0,pi/8, 0] #configuration 1
158    q3b = [0.8*pi,-0.8*pi, -pi*0.5, 0.75*pi,-pi*0.4,pi*0.4] #configuration 2
159    if 'UR' in modelName:
160        q2b[1] *= -1
161        q2b[2] *= -1
162        q3b[2] *= 0.8
163    if 'Puma560' in modelName:
164        q2b[2] *= 0.5
165        q3b[1] *= 0.1
166        q3b[2] *= 0.25
167    if 'Panda' in modelName:
168        q3b[3] *= 0.3
169        q3b[1] *= 0.5
170    if 'vx' in modelName or 'wx' in modelName or 'px' in modelName:
171        q3b[1] *= 0.5
172        q3b[2] *= 0.5
173
174    copyNJoints  = min(6, numberOfJoints )
175    q1[0:copyNJoints ] = q1b[0:copyNJoints ]
176    q2[0:copyNJoints ] = q2b[0:copyNJoints ]
177    q3[0:copyNJoints ] = q3b[0:copyNJoints ]
178
179    #trajectory generated with optimal acceleration profiles:
180    trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
181    trajectory.Add(ProfileConstantAcceleration(q1,0.25))
182    trajectory.Add(ProfileConstantAcceleration(q2,0.25))
183    trajectory.Add(ProfileConstantAcceleration(q3,0.25))
184    trajectory.Add(ProfileConstantAcceleration(q0,0.25))
185    listTrajectory.append(trajectory)
186
187
188    #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
189    #build mbs mbsRobot model:
190    robotDict = mbsRobot.CreateKinematicTree(mbs)
191    oKT = robotDict['objectKinematicTree']
192    nKT = robotDict['nodeGeneric']
193    listKinematicTree.append(oKT)
194    #if non-zero values chosen, set them here as well:
195    mbs.SetNodeParameter(nKT, 'initialCoordinates', robotDataDict['staticJointValues'])
196
197
198    #add sensors (only last sensor is kept)
199    sJointRot = mbs.AddSensor(SensorBody(bodyNumber=oKT,
200                                         storeInternal=True,
201                                         outputVariableType=exu.OutputVariableType.Coordinates))
202
203    sTorques = mbs.AddSensor(SensorBody(bodyNumber=oKT, storeInternal=True,
204                                          outputVariableType=exu.OutputVariableType.Force))
205
206#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
207#user function which is called only once per step, prescribe trajectory
208def PreStepUF(mbs, t):
209
210    for cnt, oKT in enumerate(listKinematicTree):
211        #set position and velocity in kinematic tree joints:
212        [u,v,a] = listTrajectory[cnt].Evaluate(t)
213        #note: here we would need to add the gear ratios to u and v, if they shall represent motor angles!!!
214        mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', u)
215        mbs.SetObjectParameter(oKT, 'jointVelocityOffsetVector', v)
216
217    return True
218
219mbs.SetPreStepUserFunction(PreStepUF)
220
221#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
222#assemble and simulate
223mbs.Assemble()
224
225SC.visualizationSettings.connectors.showJointAxes = True
226SC.visualizationSettings.connectors.jointAxesLength = 0.02
227SC.visualizationSettings.connectors.jointAxesRadius = 0.002
228
229SC.visualizationSettings.nodes.showBasis = True
230SC.visualizationSettings.nodes.basisSize = 0.1
231SC.visualizationSettings.loads.show = False
232SC.visualizationSettings.bodies.kinematicTree.frameSize=0.12
233SC.visualizationSettings.openGL.multiSampling=4
234
235tEnd = 1.5
236stepSize = 0.001 #could be larger!
237
238#mbs.WaitForUserToContinue()
239simulationSettings = exu.SimulationSettings() #takes currently set values or default values
240
241simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
242simulationSettings.timeIntegration.endTime = tEnd
243simulationSettings.solutionSettings.solutionWritePeriod = 0.005
244simulationSettings.solutionSettings.sensorsWritePeriod = 0.005
245simulationSettings.solutionSettings.binarySolutionFile = True
246#simulationSettings.solutionSettings.writeSolutionToFile = False
247#simulationSettings.timeIntegration.simulateInRealtime = True
248
249simulationSettings.timeIntegration.verboseMode = 1
250#simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
251
252simulationSettings.timeIntegration.newton.useModifiedNewton = True
253
254simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
255
256SC.visualizationSettings.general.drawWorldBasis = True
257SC.visualizationSettings.general.autoFitScene=False
258SC.visualizationSettings.window.renderWindowSize=[1920,1200]
259SC.visualizationSettings.openGL.perspective=1
260# SC.visualizationSettings.openGL.shadow=0.25
261SC.visualizationSettings.openGL.light0ambient = 0.5
262SC.visualizationSettings.openGL.light0diffuse = 0.5
263SC.visualizationSettings.openGL.light0position = [2,-4,8,0]
264
265useGraphics = True
266
267if useGraphics:
268    exu.StartRenderer()
269    if 'renderState' in exu.sys:
270        SC.SetRenderState(exu.sys['renderState'])
271    mbs.WaitForUserToContinue()
272
273mbs.SolveDynamic(simulationSettings,
274                 solverType=exu.DynamicSolverType.TrapezoidalIndex2,
275                 showHints=True)
276
277if useGraphics:
278    SC.visualizationSettings.general.autoFitScene = False
279    exu.StopRenderer()
280
281if True: #set True to show animation after simulation
282    mbs.SolutionViewer()
283
284
285#%%++++++++++++++++++++++++++++
286if False and not onlyShowModel:
287    mbs.PlotSensor(sensorNumbers=sJointRot, components=np.arange(numberOfJoints),
288                    title='joint angles', closeAll=True)
289    mbs.PlotSensor(sensorNumbers=sTorques, components=np.arange(numberOfJoints),
290                    title='joint torques')