serialRobotKinematicTreeDigging.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Example of a serial robot with minimal and redundant coordinates
  5#           Robot interacts with particles
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2022-12-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# import sys
 15# sys.exudynFast = True
 16
 17import exudyn as exu
 18from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 19import exudyn.graphics as graphics #only import if it does not conflict
 20#from exudyn.rigidBodyUtilities import *
 21#from exudyn.graphicsDataUtilities import *
 22from exudyn.robotics import *
 23from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
 24
 25import numpy as np
 26from numpy import linalg as LA
 27from math import pi
 28
 29SC = exu.SystemContainer()
 30mbs = SC.AddSystem()
 31
 32sensorWriteToFile = True
 33
 34#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 35compensateStaticTorques = False #static torque compensation converges slowly!
 36useKinematicTree = True
 37useGraphics = True
 38addParticles = True
 39doFast = 0 #0 / 1
 40#kinematic tree and redundant mbs agrees for stdDH version up to 1e-10, with compensateStaticTorques = False
 41# KT:      rotations at tEnd= 1.8464514676503092 , [0.4921990591981066, 0.2718999073958087, 0.818158053005264, -0.0030588904101585936, 0.26831938569719394, -0.0010660472359057434]
 42# red. MBS:rotations at tEnd= 1.8464514674961 ,   [ 0.49219906  0.27189991  0.81815805 -0.00305889  0.26831939 -0.00106605]
 43
 44
 45#cup dimensions
 46cupT = 0.005 #wall thickness
 47cupR = 0.07 #outer radius
 48cupRI = cupR-cupT #inner radius
 49cupD = 2*cupR
 50cupDI = 2*cupRI
 51cupH = 0.15  #height
 52
 53#cup offset; in TCP coordinates!
 54zOffTool = 0.2
 55xOffTool = 0.075
 56
 57tEnd = 200
 58stepSize = 0.0001#for 1000 particles
 59
 60
 61#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 62#ground box with particles
 63LL = 1
 64t = 0.02*LL
 65a = 0.2*LL
 66b = 0.35*LL #height of base
 67hw=2.4*a
 68
 69fastFact = 1
 70LLx = LL
 71if doFast:
 72    fastFact = 0.1
 73    LLx = fastFact*LL
 74
 75p0 = np.array([0.5*LL+0.5*a+0.25*LL*doFast,0,-0.5*t-b])
 76p1 = np.array([-0.5*LL,0.5*LL+0.5*a,-0.5*t-b])
 77color4wall = [0.6,0.6,0.6,0.5]
 78addNormals = False
 79gBox1 = graphics.Brick(p0,[LL,LL,t],graphics.color.steelblue,addNormals)
 80gBox1Add = graphics.Brick(p0+[-0.5*LLx,0,0.35*hw],[t,LL,0.7*hw],color4wall,addNormals)
 81gBox1 = graphics.MergeTriangleLists(gBox1, gBox1Add)
 82gBox1Add = graphics.Brick(p0+[ 0.5*LLx,0,0.5*hw],[t,LL,hw],color4wall,addNormals)
 83gBox1 = graphics.MergeTriangleLists(gBox1, gBox1Add)
 84gBox1Add = graphics.Brick(p0+[0,-0.5*LL,0.5*hw],[LLx,t,hw],color4wall,addNormals)
 85gBox1 = graphics.MergeTriangleLists(gBox1, gBox1Add)
 86gBox1Add = graphics.Brick(p0+[0, 0.5*LL,0.5*hw],[LLx,t,hw],color4wall,addNormals)
 87gBox1 = graphics.MergeTriangleLists(gBox1, gBox1Add)
 88
 89gBox2 = graphics.Brick(p1,[LL,LL,t],graphics.color.steelblue,addNormals)
 90gBox2Add = graphics.Brick(p1+[-0.5*LL,0,0.5*hw],[t,LL,hw],color4wall,addNormals)
 91gBox2 = graphics.MergeTriangleLists(gBox2, gBox2Add)
 92gBox2Add = graphics.Brick(p1+[ 0.5*LL,0,0.35*hw],[t,LL,0.7*hw],color4wall,addNormals)
 93gBox2 = graphics.MergeTriangleLists(gBox2, gBox2Add)
 94gBox2Add = graphics.Brick(p1+[0,-0.5*LL,0.35*hw],[LL,t,0.7*hw],color4wall,addNormals)
 95gBox2 = graphics.MergeTriangleLists(gBox2, gBox2Add)
 96gBox2Add = graphics.Brick(p1+[0, 0.5*LL,0.5*hw],[LL,t,hw],color4wall,addNormals)
 97gBox2 = graphics.MergeTriangleLists(gBox2, gBox2Add)
 98
 99#gDataList = [gBox1]
100
101nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0] ))
102mGround = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nGround))
103
104
105
106#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
107if addParticles:
108    np.random.seed(1) #always get same results
109
110    boxX = LL-2*t #box size for particles
111    boxY = LL-2*t
112    boxZ = a
113
114    nParticles = 12000 #50000; approx. number of particles
115    ss = max(8,int(nParticles**(1/3)*1))
116    print('tree cells x=', ss)
117    fc = 1
118    if nParticles>1000:
119        stepSize*=round((1000./nParticles)**(1./2),1)
120        if nParticles >= 80000:
121            stepSize = 1e-5
122        if nParticles >= 80000*2:
123            stepSize = 5e-6
124        if nParticles <= 12000:
125            stepSize = 5e-5
126
127        if stepSize <= 2e-5:
128            fc = 4
129
130    print('step size=',stepSize)
131
132    npx = int(nParticles**(1./3.)) #approx particles in one dimension
133    radius0 = boxX/(npx*2+1.5)*0.499
134    print('LL=',LL,',npx=',npx,',r=',radius0)
135    npz = int(npx*0.75) #0.75
136    npx *= 2
137
138    gContact = mbs.AddGeneralContact()
139    gContact.verboseMode = 1
140    gContact.resetSearchTreeInterval = 10000 #interval at which search tree memory is cleared
141    frictionCoeff = 0
142    gContact.SetFrictionPairings(frictionCoeff*np.eye(1))
143    gContact.SetSearchTreeCellSize(numberOfCells=[ss,ss,ss])
144    #gContact.SetSearchTreeBox([0,-1,-0.1],[1.1,1,0.5])
145    #gContact.SetSearchTreeBox([0,-2,0],[0.5*LL,0.5*LL,2])
146
147    #contact parameters:
148    k = 2e4*4
149    d = 0.002*k #damping also has influence on conservation of (angular) momentum; improved if multiplied with factor 0.05
150    density = 1000
151    m = density*4/3*pi*radius0**3 #all particles get same mass!
152    m /= radius0 #use larger mass for smaller particles ...
153
154    if addParticles:
155        [meshPoints, meshTrigs] = graphics.ToPointsAndTrigs(gBox1)
156        gContact.AddTrianglesRigidBodyBased(rigidBodyMarkerIndex=mGround,
157                                            contactStiffness=k, contactDamping=d, frictionMaterialIndex=0,
158            pointList=meshPoints,  triangleList=meshTrigs)
159        [meshPoints, meshTrigs] = graphics.ToPointsAndTrigs(gBox2)
160        gContact.AddTrianglesRigidBodyBased(rigidBodyMarkerIndex=mGround,
161                                            contactStiffness=k, contactDamping=d, frictionMaterialIndex=0,
162            pointList=meshPoints,  triangleList=meshTrigs)
163
164    #create particles:
165    color4node = graphics.color.blue
166    cnt = 0
167    pBoxRef = p0 + [-0.5*radius0,-0.5*radius0,t+radius0]
168
169    npy = npx
170    if doFast:
171        npx = int(fastFact*npx-2.5)
172
173    for ix in range(npx+1):
174        for iy in range(npy+1):
175            for iz in range(npz+1):
176
177                color4node = graphics.colorList[int(min((iz/npz*10),10) )]
178
179                valueRand = np.random.random(1)[0]
180                radius = radius0 - radius0*0.3*valueRand #add some random size to decrease artifacts
181
182
183                pX = (iz%2)*radius0 #create densly packed particles
184                pY = (iz%2)*radius0
185                pRef0 = [(ix-npx*0.5)*radius0*2+pX,
186                         (iy-npy*0.5)*radius0*2+pY,
187                         0.73*iz*radius0*2-0.5*t]
188                # print(pRef0)
189                pRef = np.array(pRef0) + pBoxRef
190                v0 = [0,0,0]
191
192                if (cnt%20000 == 0): print("create mass",cnt)
193                nMass = mbs.AddNode(NodePoint(referenceCoordinates=pRef,
194                                              initialVelocities=v0,
195                                              visualization=VNodePoint(show=True,drawSize=2*radius, color=color4node)))
196
197                #omitting the graphics speeds up, but does not allow shadow of particles ...
198                oMass = mbs.AddObject(MassPoint(physicsMass=m, nodeNumber=nMass,
199                                                #visualization=VMassPoint(graphicsData=[graphics.Sphere(radius=radius, color=color4node, nTiles=6)])
200                                                ))
201                mThis = mbs.AddMarker(MarkerNodePosition(nodeNumber=nMass))
202
203                mbs.AddLoad(Force(markerNumber=mThis, loadVector= [0,0,-m*9.81]))
204
205                gContact.AddSphereWithMarker(mThis, radius=radius,
206                                             contactStiffness=k, contactDamping=d, frictionMaterialIndex=0)
207
208                cnt += 1
209    print('total particles added=', cnt)
210
211gCup=[]
212if True: #add cup
213    colorCup = [0.8,0.1,0.1,0.5]
214    contour=np.array([[0,0],[0,cupR],[cupH,cupR],[cupH, cupR-cupT],[cupT, cupR-cupT],[cupT, 0]])
215    contour = list(contour)
216    contour.reverse()
217    gCup = graphics.SolidOfRevolution(pAxis=[xOffTool,0,zOffTool], vAxis=[-1,0,0],
218            contour=contour, color=colorCup, nTiles = 64)
219
220    gCupAdd = graphics.Cylinder(pAxis=[0,0,0], vAxis=[0,0,zOffTool-cupRI*1.01], radius=0.02, color=colorCup)
221    gCup = graphics.MergeTriangleLists(gCup, gCupAdd)
222
223    [meshPointsTool, meshTrigsTool] = graphics.ToPointsAndTrigs(gCup)
224
225
226
227from exudyn.robotics.models import ManipulatorPuma560, ManipulatorUR5
228
229robotDef = ManipulatorPuma560() #get dictionary that defines kinematics
230
231robotDef['links'][0]['inertia']=np.diag([1e-4,0.35,1e-4])
232#print(robotDef)
233Pcontrol = fc* np.array([40000*fc, 40000*fc, 40000*fc, 100*fc, 100*fc, 10*fc])
234Dcontrol = fc* np.array([400*fc,   400*fc,   100*fc,   1*fc,   1*fc,   0.1*fc])
235
236pBase=[0,0,0]
237gravity=[0,0,-9.81]  #gravity
238
239graphicsBaseList  = []
240graphicsBaseList += [graphics.Brick([0,0,-b*0.5-0.025], [a,a,b+t-0.05], graphics.color.brown)]
241graphicsBaseList += [graphics.CheckerBoard([0,0,-b-0.5*t], size=2.4)]
242
243
244rRobotTCP = 0.041
245graphicsToolList = [graphics.Cylinder(pAxis=[0,0,0], vAxis= [0,0,0.06], radius=0.05, color=graphics.color.red, nTiles=8)]
246
247
248graphicsToolList+= [gCup]
249
250
251#changed to new robot structure July 2021:
252robot = Robot(gravity=gravity,
253              base = RobotBase(HT=HTtranslate(pBase), visualization=VRobotBase(graphicsData=graphicsBaseList)),
254              tool = RobotTool(HT=HTtranslate([0,0,0]), visualization=VRobotTool(graphicsData=graphicsToolList)),
255              referenceConfiguration = []) #referenceConfiguration created with 0s automatically
256
257
258
259for cnt, link in enumerate(robotDef['links']):
260    robot.AddLink(RobotLink(mass=link['mass'],
261                               COM=link['COM'],
262                               inertia=link['inertia'],
263                               localHT=StdDH2HT(link['stdDH']),
264                               PDcontrol=(Pcontrol[cnt], Dcontrol[cnt]),
265                               visualization=VRobotLink(linkColor=graphics.colorList[cnt], showCOM=False, showMBSjoint=useGraphics)
266                               ))
267
268#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
269#configurations and trajectory
270q0 = [0,0.5*pi,-1.0*pi,0,0,0] #zero angle configuration
271
272q1 = [-0.07*pi,0.20*pi,-0.8*pi,0, 0.0*pi,-0.9*pi]
273q2 = [-0.07*pi,0.16*pi,-0.9*pi,0, 0.0*pi,-0.6*pi]
274q3 = [ 0.10*pi,0.16*pi,-0.9*pi,0, 0.0*pi,-0.4*pi]
275q4 = [ 0.10*pi,0.40*pi,-1.0*pi,0,0.15*pi,-0.15*pi]
276q5 = [ 0.65*pi,0.40*pi,-1.0*pi,0,0.15*pi, 0.15*pi]
277q6 = [ 0.65*pi,0.30*pi,-0.9*pi,0, 0.0*pi,-1*pi]
278q7 = [ 0.65*pi,0.40*pi,-0.9*pi,0, 0.0*pi,-1*pi]
279
280doFast2 = 1*doFast
281
282if doFast2:
283    q1 = [-0.07*pi,0.16*pi,-0.8*pi,0, 0.0*pi,-0.9*pi]  #fast trajectory
284
285#trajectory generated with optimal acceleration profiles:
286trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
287# trajectory.Add(ProfileConstantAcceleration(q0,0.1))
288trajectory.Add(ProfileConstantAcceleration(q1,0.25*(1-0.8*doFast2)))
289# trajectory.Add(ProfileConstantAcceleration(q1,0.5))
290trajectory.Add(ProfileConstantAcceleration(q2,0.5*(1-0.9*doFast2)))
291# trajectory.Add(ProfileConstantAcceleration(q2,0.5))
292trajectory.Add(ProfileConstantAcceleration(q3,0.5*(1-0.9*doFast2)))
293# trajectory.Add(ProfileConstantAcceleration(q3,0.5))
294trajectory.Add(ProfileConstantAcceleration(q4,0.5*1.5))
295# trajectory.Add(ProfileConstantAcceleration(q4,0.5))
296trajectory.Add(ProfileConstantAcceleration(q5,0.5*1.5))
297#trajectory.Add(ProfileConstantAcceleration(q5,0.5))
298trajectory.Add(ProfileConstantAcceleration(q6,0.30))
299trajectory.Add(ProfileConstantAcceleration(q7,0.15))
300
301trajectory.Add(ProfileConstantAcceleration(q0,0.25))
302
303# x = traj.EvaluateCoordinate(t,0)
304
305
306#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
307#test robot model
308#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
309
310
311jointList = [0]*robot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
312
313def ComputeMBSstaticRobotTorques(robot):
314
315    if not useKinematicTree:
316        q=[]
317        for joint in jointList:
318            q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
319    else:
320        q = mbs.GetObjectOutputBody(oKT, exu.OutputVariableType.Coordinates, localPosition=[0,0,0])
321
322    HT=robot.JointHT(q)
323    return robot.StaticTorques(HT)
324
325#++++++++++++++++++++++++++++++++++++++++++++++++
326#base, graphics, object and marker:
327
328objectGround = mbs.AddObject(ObjectGround(referencePosition=HT2translation(robot.GetBaseHT()),
329                                      #visualization=VObjectGround(graphicsData=graphicsBaseList)
330                                          ))
331
332
333#baseMarker; could also be a moving base!
334baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
335
336
337#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
338#build mbs robot model:
339if True:
340    robotDict = robot.CreateKinematicTree(mbs)
341    oKT = robotDict['objectKinematicTree']
342
343    mbs.SetNodeParameter(robotDict['nodeGeneric'],'initialCoordinates',q0) #set according initial coordinates
344
345    sTCP = mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=5, localPosition=[xOffTool,0,zOffTool],
346                                             storeInternal=True, outputVariableType=exu.OutputVariableType.Position))
347
348    mTCP = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=5, localPosition=[0,0,0]))
349
350    if addParticles:
351        gContact.AddTrianglesRigidBodyBased(rigidBodyMarkerIndex=mTCP, contactStiffness=k, contactDamping=d, frictionMaterialIndex=0,
352            pointList=meshPointsTool,  triangleList=meshTrigsTool)
353
354    #add ground after robot, to enable transparency
355    oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
356                                       visualization=VObjectGround(graphicsData=[gBox1,gBox2])))
357
358    tMax = trajectory.GetTimes()[-1] #total trajectory time
359    print('trajectory cycle time=',round(tMax))
360    #user function which is called only once per step, speeds up simulation drastically
361    def PreStepUF(mbs, t):
362        if compensateStaticTorques:
363            staticTorques = ComputeMBSstaticRobotTorques(robot)
364            #print("tau=", staticTorques)
365        else:
366            staticTorques = np.zeros(len(jointList))
367
368        tCnt = int(t/tMax)
369        tOff = tCnt*tMax
370        [u,v,a] = trajectory.Evaluate(t-tOff)
371
372        #in case of kinematic tree, very simple operations!
373        mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', u)
374        mbs.SetObjectParameter(oKT, 'jointVelocityOffsetVector', v)
375        mbs.SetObjectParameter(oKT, 'jointForceVector', staticTorques)
376
377        return True
378
379    mbs.SetPreStepUserFunction(PreStepUF)
380
381
382mbs.Assemble()
383#mbs.systemData.Info()
384
385SC.visualizationSettings.connectors.showJointAxes = True
386SC.visualizationSettings.connectors.jointAxesLength = 0.02
387SC.visualizationSettings.connectors.jointAxesRadius = 0.002
388
389SC.visualizationSettings.nodes.showBasis = False
390SC.visualizationSettings.loads.show = False
391
392SC.visualizationSettings.openGL.multiSampling=4
393
394
395#mbs.WaitForUserToContinue()
396simulationSettings = exu.SimulationSettings() #takes currently set values or default values
397
398simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
399simulationSettings.timeIntegration.endTime = tEnd
400simulationSettings.timeIntegration.stepInformation = 1+32 #time to go and time spent
401simulationSettings.solutionSettings.solutionWritePeriod = 0.01*2
402simulationSettings.solutionSettings.sensorsWritePeriod = 0.005
403simulationSettings.solutionSettings.binarySolutionFile = True
404simulationSettings.solutionSettings.outputPrecision = 5 #make files smaller
405simulationSettings.solutionSettings.exportAccelerations = False
406simulationSettings.solutionSettings.exportVelocities = False
407simulationSettings.solutionSettings.coordinatesSolutionFileName = 'solution/test.sol'
408#simulationSettings.solutionSettings.writeSolutionToFile = False
409# simulationSettings.timeIntegration.simulateInRealtime = True
410# simulationSettings.timeIntegration.realtimeFactor = 0.25
411simulationSettings.timeIntegration.explicitIntegration.computeEndOfStepAccelerations = False #speedup ...
412simulationSettings.timeIntegration.explicitIntegration.computeMassMatrixInversePerBody = True #>>speedup ...
413# simulationSettings.timeIntegration.reuseConstantMassMatrix = True
414
415simulationSettings.parallel.numberOfThreads = 8
416
417simulationSettings.timeIntegration.verboseMode = 1
418simulationSettings.timeIntegration.verboseModeFile = 1
419simulationSettings.solutionSettings.solverInformationFileName = 'solution/solverTest.txt'
420# simulationSettings.displayComputationTime = True
421# simulationSettings.displayStatistics = True
422simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
423
424#simulationSettings.timeIntegration.newton.useModifiedNewton = True
425simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
426simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
427simulationSettings.timeIntegration.newton.useModifiedNewton = True
428
429simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
430SC.visualizationSettings.general.autoFitScene=False
431SC.visualizationSettings.window.renderWindowSize=[1920,1200]
432#SC.visualizationSettings.general.circleTiling = 100
433SC.visualizationSettings.general.textSize = 14
434SC.visualizationSettings.general.showSolutionInformation = False
435SC.visualizationSettings.general.showSolverInformation = False
436SC.visualizationSettings.general.graphicsUpdateInterval = 4#0.05
437SC.visualizationSettings.bodies.kinematicTree.showJointFrames=False
438SC.visualizationSettings.general.drawCoordinateSystem=False
439SC.visualizationSettings.general.drawWorldBasis=False
440
441SC.visualizationSettings.nodes.drawNodesAsPoint = False
442SC.visualizationSettings.nodes.show = True
443SC.visualizationSettings.markers.show = False
444SC.visualizationSettings.nodes.defaultSize = 0 #must not be -1, otherwise uses autocomputed size
445SC.visualizationSettings.nodes.tiling = 8
446SC.visualizationSettings.openGL.shadow = 0.4
447# SC.visualizationSettings.contact.showSearchTree = 1
448# SC.visualizationSettings.contact.showSearchTreeCells = 1
449
450if useGraphics:
451    exu.StartRenderer()
452    if 'renderState' in exu.sys:
453        SC.SetRenderState(exu.sys['renderState'])
454    mbs.WaitForUserToContinue()
455
456# pTCP = mbs.GetSensorValues(sTCP)
457# print('pTCP=',pTCP)
458#mbs.SolveDynamic(simulationSettings, showHints=True)
459mbs.SolveDynamic(simulationSettings,
460                  #solverType=exu.DynamicSolverType.RK44,
461                  solverType=exu.DynamicSolverType.ExplicitEuler,
462                  showHints=True)
463
464
465if useGraphics:
466    SC.visualizationSettings.general.autoFitScene = False
467    exu.StopRenderer()
468
469if True:
470#%%++++++++++
471    SC.visualizationSettings.general.autoFitScene = False
472    # SC.visualizationSettings.general.graphicsUpdateInterval=0.5
473
474    # print('load solution file')
475    # sol = LoadSolutionFile('solution/test.sol', safeMode=True)#, maxRows=100)
476    # print('start SolutionViewer')
477    # mbs.SolutionViewer(sol)
478    mbs.SolutionViewer()