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