humanRobotInteraction.py
You can view and download this file on Github: humanRobotInteraction.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: 3D rigid body tutorial with 2 bodies and revolute joints, using new utilities functions
5#
6# Author: Johannes Gerstmayr
7# Date: 2021-08-05
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
13import exudyn as exu
14from exudyn.itemInterface import *
15from exudyn.utilities import * #includes graphics and rigid body utilities
16import numpy as np
17from exudyn.robotics import *
18from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
19
20from math import pi
21import copy
22import time
23
24SC = exu.SystemContainer()
25mbs = SC.AddSystem()
26
27addRobot = True
28useKT = True #model articulated body as kinematic tree; allows simpler control
29addHand = True
30addHandKinematic = True and addHand
31addFullBody = True #only graphics
32
33
34tStart = time.time()
35
36#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
37#physical parameters
38gravity=[0,0,-9.81] #gravity
39L = 1 #length
40w = 0.1 #width
41bodyDim=[L,w,w] #body dimensions
42p0 = [0,0,0] #origin of pendulum
43pMid0 = np.array([L*0.5*0,0,0]) #center of mass, body0
44
45#ground body
46
47#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
48
49scaleBody = 0.0254 #conversion: inches to meter (articulated-dummy), original file
50scaleBody = 0.001 #conversion: millimeter to meter (articulated-dummy2), modified file
51
52leftShoulder = np.array([0.086, -0.185, 1.383]) #position of left shoulder joint in meter
53leftElbow = np.array([0.109, -0.206, 1.122]) #position of left shoulder joint in meter
54leftHand = np.array([0.0945, -0.2520, 0.8536]) #position of left shoulder joint in meter
55
56rightShoulder = np.array([0.086, 0.185, 1.383]) #position of left shoulder joint in meter
57rightElbow = np.array([0.109, 0.206, 1.122]) #position of left shoulder joint in meter
58rShoulder = 0.047
59rElbow = 0.032
60rHand = 0.025
61
62density = 1000. #human body average density ... like water
63verbose = False
64graphicsBody =[]
65graphicsUAL =[]
66graphicsLAL =[]
67
68listBody = ['UpperArm_R', 'LowerArm_R', 'Hand_R', 'Fingers_R', 'Head', 'Pelvis',
69 'LowerLeg_R', 'LowerLeg_L', 'UpperLeg_R', 'UpperLeg_L', 'Foot_L', 'Foot_R']
70
71listAll = ['Torso', 'UpperArm_L', 'LowerArm_L', 'Hand_L', 'Fingers_L'] + listBody
72myDir = 'C:/DATA/cpp/DocumentationAndInformation/STL/articulated-dummy2/' #graphics for articulated dummy not included in model; download at GrabCAD / articulated-dummy
73
74
75if False:
76 #convert ascii files to binary stl:
77 from stl import mesh, Mode
78 for file in ['Fingers_L']:
79 #for file in listAll:
80 print('convert',file)
81 data=mesh.Mesh.from_file(myDir+file+'.stl')
82 data.save(filename=myDir+file+'.stl', mode=Mode.BINARY)
83
84
85#graphics taken from https://grabcad.com/library/articulated-dummy-1
86dataUAL = GraphicsDataFromSTLfile(fileName=myDir+'UpperArm_L.stl',
87 color=color4blue, verbose=verbose, density=density,
88 scale = scaleBody)
89
90dataLAL = GraphicsDataFromSTLfile(fileName=myDir+'LowerArm_L.stl',
91 color=color4dodgerblue, verbose=verbose, density=density,
92 scale = scaleBody)
93
94if addHand:
95 dataHandL = GraphicsDataFromSTLfile(fileName=myDir+'Hand_L.stl',
96 color=color4brown, verbose=verbose, density=density,
97 scale = scaleBody)
98 dataFingersL = GraphicsDataFromSTLfile(fileName=myDir+'Fingers_L.stl',
99 color=color4brown, verbose=verbose, density=density,
100 scale = scaleBody)
101
102 #++++++++++++++++++++++++++++++++++
103 #merge mass, COM and inertia of lower arm, hand and fingers:
104 rbiLAL = RigidBodyInertia(dataLAL[1]['mass'], dataLAL[1]['inertia'], dataLAL[1]['COM'], inertiaTensorAtCOM=True)
105 rbiHandL = RigidBodyInertia(dataHandL[1]['mass'], dataHandL[1]['inertia'], dataHandL[1]['COM'], inertiaTensorAtCOM=True)
106 rbiFingersL = RigidBodyInertia(dataFingersL[1]['mass'], dataFingersL[1]['inertia'], dataFingersL[1]['COM'], inertiaTensorAtCOM=True)
107
108 # print('rbiLAL=',rbiLAL)
109 # print('rbiHandL=',rbiHandL)
110 # print('rbiFingersL=',rbiFingersL)
111 rbiLAL= rbiLAL + rbiHandL + rbiFingersL
112 dataLAL[1]['mass'] = rbiLAL.Mass()
113 dataLAL[1]['inertia'] = rbiLAL.InertiaCOM()
114 dataLAL[1]['COM'] = rbiLAL.COM()
115#++++++++++++++++++++++++++++++++++
116graphicsBody += [AddEdgesAndSmoothenNormals(GraphicsDataFromSTLfile(fileName=myDir+'Torso.stl',
117 color=color4grey, verbose=verbose, density=density,
118 scale = scaleBody)[0], addEdges=False)]
119
120
121if addFullBody:
122 for part in listBody:
123 data = GraphicsDataFromSTLfile(fileName=myDir+''+part+'.stl',
124 color=color4grey, verbose=verbose, density=density*0,
125 scale = scaleBody)
126 graphicsBody += [AddEdgesAndSmoothenNormals(data, addEdges=False)]
127 #graphicsBody += [data[0]]
128
129
130# if True: #makes even bigger files ...
131 # fileName = myDir+'data.npy'
132 # with open(fileName, 'wb') as f:
133 # np.save(f, graphicsBody, allow_pickle=True)
134
135 # with open(fileName, 'rb') as f:
136 # graphicsBody = np.load(f, allow_pickle=True).all()
137
138
139#body fixed to ground
140graphicsBody += [GraphicsDataCheckerBoard(size=4)]
141
142pBody = np.array([0,0,0])
143oGround = mbs.AddObject(ObjectGround(referencePosition=pBody, visualization=VObjectGround(graphicsData=graphicsBody)))
144
145if useKT:
146
147 #%%++++++++++++++++++++++++++++++++++++++++
148 #motion and control of articulated body
149 z0 = -0.15*pi
150 q0 = [0,0,0,0,0,0,0] #zero angle configuration, max 7 joints
151 q1 = [0,pi*0.125,z0,pi*(0.375-0.03),0,0,0] #zero angle configuration, max 7 joints
152 # q1 = [-pi*0.5,0,0,0,0,0,0]
153 q2 = [-pi*0.5,pi*0.5,0,0,0,0,0]
154 q3 = [-pi*0.5,pi*0.5,pi*0.5,0,0,0,0]
155 q4 = [0,pi*0.125,0,pi*0.375,0,0,0]
156
157 #trajectory generated with optimal acceleration profiles:
158 bodyTrajectory = Trajectory(initialCoordinates=q0, initialTime=0.25)
159 bodyTrajectory.Add(ProfileConstantAcceleration(q1,0.4))
160 #bodyTrajectory.Add(ProfileConstantAcceleration(q1,0.25))
161 # bodyTrajectory.Add(ProfileConstantAcceleration(q2,0.25))
162 # bodyTrajectory.Add(ProfileConstantAcceleration(q3,0.25))
163 # bodyTrajectory.Add(ProfileConstantAcceleration(q4,0.25))
164 # bodyTrajectory.Add(ProfileConstantAcceleration(q0,0.25))
165
166 Pcontrol = 0.1*np.array([5000,2*5000,5000, 2*5000, 2000,2000,2000]) #3 x elbow, 1 x shoulder, 3 x hand
167 Dcontrol = 0.02 * Pcontrol
168 #%%++++++++++++++++++++++++++++++++++++++++
169
170 articulatedBody = Robot(gravity=[0,0,9.81],
171 #base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)), #already added to ground
172 #tool = RobotTool(HT=HTtranslate([0,0,0]), visualization=VRobotTool(graphicsData=graphicsToolList)),
173 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
174
175 jointRadius = 0.06
176 jointWidth = 0.0125
177 linkWidth = 0.001
178 showMBSjoint= False
179 showCOM = False
180
181 body = dataUAL
182 body[0] = MoveGraphicsData(AddEdgesAndSmoothenNormals(body[0], addEdges=False), -leftShoulder, np.eye(3))
183 link = body[1]
184
185 articulatedBody.AddLink(RobotLink(jointType='Rx',
186 mass = 0, COM=[0,0,0], inertia = 0*np.eye(3),
187 preHT=HomogeneousTransformation(np.eye(3), leftShoulder),
188 PDcontrol=(Pcontrol[0], Dcontrol[0]),
189 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint)
190 ))
191 articulatedBody.AddLink(RobotLink(jointType='Ry',
192 mass = 0, COM=[0,0,0], inertia = 0*np.eye(3),
193 #preHT=HT0(),
194 PDcontrol=(Pcontrol[1], Dcontrol[1]),
195 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint)
196 ))
197
198
199 articulatedBody.AddLink(RobotLink(jointType='Rz',
200 mass=link['mass'],
201 COM=link['COM']-leftShoulder,
202 inertia=link['inertia'],
203 #preHT=HomogeneousTransformation(np.eye(3), leftShoulder),
204 PDcontrol=(Pcontrol[2], Dcontrol[2]),
205 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint,
206 graphicsData=[body[0]])
207 ))
208
209 body = dataLAL
210 body[0] = MoveGraphicsData(AddEdgesAndSmoothenNormals(body[0], addEdges=False), -leftElbow, np.eye(3))
211 link = body[1]
212 gList = [body[0]]
213
214 if addHand:
215 if not addHandKinematic:
216 dataHandL[0] = MoveGraphicsData(AddEdgesAndSmoothenNormals(dataHandL[0], addEdges=False), -leftElbow, np.eye(3))
217 dataFingersL[0] = MoveGraphicsData(dataFingersL[0], -leftElbow, np.eye(3))
218 gList = [body[0], dataHandL[0], dataFingersL[0]]
219
220
221 gList += [GraphicsDataSphere(leftHand-leftElbow, radius=rHand, color=color4brown, nTiles=16)]
222
223 articulatedBody.AddLink(RobotLink(jointType='Ry',
224 mass=link['mass'],
225 COM=link['COM']-leftElbow,
226 inertia=link['inertia'],
227 preHT=HomogeneousTransformation(np.eye(3), leftElbow-leftShoulder),
228 PDcontrol=(Pcontrol[3], Dcontrol[3]),
229 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint,
230 graphicsData=gList)
231 ))
232
233 if addHandKinematic:
234 body = dataHandL
235 link = body[1]
236 dataHandL[0] = MoveGraphicsData(dataHandL[0], -leftHand, np.eye(3))
237 dataFingersL[0] = MoveGraphicsData(dataFingersL[0], -leftHand, np.eye(3))
238 gList = [dataHandL[0], dataFingersL[0]]
239
240
241 articulatedBody.AddLink(RobotLink(jointType='Rx',
242 mass = 0, COM=[0,0,0], inertia = 0*np.eye(3),
243 preHT=HomogeneousTransformation(np.eye(3), leftHand-leftElbow),
244 PDcontrol=(Pcontrol[4], Dcontrol[4]),
245 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint)
246 ))
247 articulatedBody.AddLink(RobotLink(jointType='Ry',
248 mass = 0, COM=[0,0,0], inertia = 0*np.eye(3),
249 PDcontrol=(Pcontrol[5], Dcontrol[5]),
250 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint)
251 ))
252 articulatedBody.AddLink(RobotLink(jointType='Rz',
253 mass=link['mass'],
254 COM=link['COM']-leftShoulder,
255 inertia=link['inertia'],
256 PDcontrol=(Pcontrol[6], Dcontrol[6]),
257 visualization=VRobotLink(showCOM=showCOM, jointRadius=jointRadius, jointWidth=jointWidth, linkWidth=linkWidth, showMBSjoint=showMBSjoint,
258 graphicsData=gList)
259 ))
260
261
262 articulatedBody.referenceConfiguration = q0[0:articulatedBody.NumberOfLinks()]
263 #create kinematic tree of body
264 articulatedBodyDict = articulatedBody.CreateKinematicTree(mbs)
265 oKTarticulatedBody = articulatedBodyDict['objectKinematicTree']
266
267 mHand = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber = oKTarticulatedBody, linkNumber = 3,
268 localPosition=leftHand-leftElbow))
269
270else:
271 #%%++++++++++++++++++++++++++
272 #upper arm left:
273 if False:
274 body = dataUAL
275
276 body[0] = MoveGraphicsData(body[0], -body[1]['COM'], np.eye(3))
277
278 [nUAL,bUAL]=AddRigidBody(mainSys = mbs,
279 inertia = RigidBodyInertia(mass=body[1]['mass'], inertiaTensor=body[1]['inertia'], com=[0,0,0]),
280 nodeType = exu.NodeType.RotationEulerParameters,
281 position = body[1]['COM'],
282 rotationMatrix = np.eye(3),
283 angularVelocity = [0,2*0,0],
284 gravity = gravity,
285 graphicsDataList = [body[0]])
286
287 #markers for ground and rigid body (not needed for option 3):
288 markerGroundUAL = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=leftShoulder))
289 markerUAL0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUAL, localPosition=leftShoulder-body[1]['COM']))
290 markerUAL1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUAL, localPosition=leftElbow-body[1]['COM']))
291
292 jUAL = mbs.AddObject(SphericalJoint(markerNumbers=[markerGroundUAL, markerUAL0],
293 visualization=VSphericalJoint(jointRadius=rShoulder)))
294
295
296 #%%++++++++++++++++++++++++++
297 #lower arm left:
298 body = dataLAL
299
300 body[0] = MoveGraphicsData(body[0], -body[1]['COM'], np.eye(3))
301
302 [nUAL,bUAL]=AddRigidBody(mainSys = mbs,
303 inertia = RigidBodyInertia(mass=body[1]['mass'], inertiaTensor=body[1]['inertia'], com=[0,0,0]),
304 nodeType = exu.NodeType.RotationEulerParameters,
305 position = body[1]['COM'],
306 rotationMatrix = np.eye(3),
307 angularVelocity = [0,2*0,0],
308 gravity = gravity,
309 graphicsDataList = [body[0]])
310
311 #markers for ground and rigid body (not needed for option 3):
312 markerLAL0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUAL, localPosition=leftElbow-body[1]['COM']))
313
314 jLAL = mbs.AddObject(GenericJoint(markerNumbers=[markerUAL1, markerLAL0],
315 constrainedAxes=[1,1,1, 1,0,1],
316 visualization=VGenericJoint(axesRadius=0.1*rElbow)))
317
318#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
319#add simple robot:
320
321if addRobot:
322 from exudyn.robotics.models import ManipulatorPuma560, ManipulatorUR5
323
324 robotDef = ManipulatorPuma560() #get dictionary that defines kinematics
325 fc = 0.5
326 Pcontrol = fc* np.array([40000, 40000, 40000, 100, 100, 10])
327 Dcontrol = fc* np.array([400, 400, 100, 1, 1, 0.1])
328
329 pBase = np.array([-1,-0.2,0.75])
330 #+++++++++
331 #some graphics for Puma560
332 jointWidth=0.1
333 jointRadius=0.06
334 linkWidth=0.1
335
336 graphicsBaseList = [GraphicsDataOrthoCubePoint([0,0,-0.15], [0.12,0.12,0.1], color4grey)]
337 graphicsBaseList = [GraphicsDataOrthoCubePoint([0,0,-0.75*0.5-0.05], [pBase[2],pBase[2],0.65], color4brown)]
338 graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0.5,0,0], 0.0025, color4red)]
339 graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0.5,0], 0.0025, color4green)]
340 graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0,0.5], 0.0025, color4blue)]
341 graphicsBaseList +=[GraphicsDataCylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, color4list[0])] #belongs to first body
342
343 rRobotTCP = 0.041 #also used for contact
344 ty = 0.03
345 tz = 0.04
346 zOff = -0.05+0.1
347 toolSize= [0.05,0.5*ty,0.06]
348 graphicsToolList = [GraphicsDataCylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=color4red)]
349 # graphicsToolList+= [GraphicsDataOrthoCubePoint([0,ty,1.5*tz+zOff], toolSize, color4grey)] #gripper
350 # graphicsToolList+= [GraphicsDataOrthoCubePoint([0,-ty,1.5*tz+zOff], toolSize, color4grey)] #gripper
351 graphicsToolList+= [GraphicsDataSphere(point=[0,0,0.12],radius=rRobotTCP, color=[1,0,0,1], nTiles=16)]
352
353 #+++++++++
354
355 #changed to new robot structure July 2021:
356 robot = Robot(gravity=gravity,
357 base = RobotBase(HT=HTtranslate(pBase), visualization=VRobotBase(graphicsData=graphicsBaseList)),
358 tool = RobotTool(HT=HTtranslate([0,0,0]), visualization=VRobotTool(graphicsData=graphicsToolList)),
359 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
360
361 for cnt, link in enumerate(robotDef['links']):
362 robot.AddLink(RobotLink(mass=link['mass'],
363 COM=link['COM'],
364 inertia=link['inertia'],
365 localHT=StdDH2HT(link['stdDH']),
366 PDcontrol=(Pcontrol[cnt], Dcontrol[cnt]),
367 visualization=VRobotLink(linkColor=color4list[cnt], showCOM=False, showMBSjoint=True)
368 ))
369
370 q0 = [0,0.5*pi,-0.5*pi,0,0,0] #zero angle configuration
371
372 q1 = [-0.35*pi,0.5*pi,-0.5*pi,0,0,0] #zero angle configuration
373 q2 = [-0.35*pi,0.5*pi,-1.0*pi,0,0,0] #zero angle configuration
374 q3 = [-0.35*pi,0.25*pi,-0.75*pi,0,0,0] #zero angle configuration
375 q4 = [ 0.07*pi,0.38*pi,-0.88*pi,0,0,0] #zero angle configuration
376
377 robot.referenceConfiguration = q0
378
379 #trajectory generated with optimal acceleration profiles:
380 robotTrajectory = Trajectory(initialCoordinates=q0, initialTime=0.)
381 robotTrajectory.Add(ProfileConstantAcceleration(q0,0.25))
382 # robotTrajectory.Add(ProfileConstantAcceleration(q1,0.25))
383 # robotTrajectory.Add(ProfileConstantAcceleration(q2,0.5))
384 robotTrajectory.Add(ProfileConstantAcceleration(q3,0.25))
385 robotTrajectory.Add(ProfileConstantAcceleration(q4,0.2)) #0.25 is regular speed
386
387 robotDict = robot.CreateKinematicTree(mbs)
388 oKTrobot = robotDict['objectKinematicTree']
389
390 mRobotTCP0 = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber = oKTrobot, linkNumber = 5,
391 localPosition=[0,0,0.12]))
392 # mRobotTCP1 = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber = oKTrobot, linkNumber = 5,
393 # localPosition=[0,0,0.12]))
394 # mRobotTCP2 = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber = oKTrobot, linkNumber = 5,
395 # localPosition=[0,0,0.14]))
396
397def PreStepUF(mbs, t):
398 if useKT:
399 [u,v,a] = bodyTrajectory.Evaluate(t)
400
401 #in case of kinematic tree, very simple operations!
402 mbs.SetObjectParameter(oKTarticulatedBody, 'jointPositionOffsetVector', u)
403 mbs.SetObjectParameter(oKTarticulatedBody, 'jointVelocityOffsetVector', v)
404 # if compensateStaticTorques:
405 # mbs.SetObjectParameter(oKTarticulatedBody, 'jointForceVector', ComputeMBSstaticRobotTorques(articulatedBody))
406 if addRobot:
407 [u,v,a] = robotTrajectory.Evaluate(t)
408
409 #in case of kinematic tree, very simple operations!
410 mbs.SetObjectParameter(oKTrobot, 'jointPositionOffsetVector', u)
411 mbs.SetObjectParameter(oKTrobot, 'jointVelocityOffsetVector', v)
412 # if compensateStaticTorques:
413 # mbs.SetObjectParameter(oKTrobot, 'jointForceVector', ComputeMBSstaticRobotTorques(articulatedBody))
414
415 return True
416
417mbs.SetPreStepUserFunction(PreStepUF)
418
419print('loading took',time.time()-tStart,'seconds')
420
421
422if True:
423 markerList = [mHand, mRobotTCP0]
424 radiusList = [rHand*1.1, rRobotTCP]
425
426 # rr=0.3
427 # [n0,b0]=AddRigidBody(mainSys = mbs,
428 # inertia = RigidBodyInertia(mass=100, inertiaTensor=np.eye(3), com=[0,0,0]),
429 # nodeType = exu.NodeType.RotationEulerParameters,
430 # position = [-0.3,-0.1,2],
431 # rotationMatrix = np.eye(3),
432 # angularVelocity = [0,0,0],
433 # gravity = gravity,
434 # graphicsDataList = [GraphicsDataSphere(radius=rr, color=color4green, nTiles=16)])
435 # markerList += [mbs.AddMarker(MarkerNodeRigid(nodeNumber=n0))]
436 # radiusList += [rr]
437 # [n0,b0]=AddRigidBody(mainSys = mbs,
438 # inertia = RigidBodyInertia(mass=100, inertiaTensor=np.eye(3), com=[0,0,0]),
439 # nodeType = exu.NodeType.RotationEulerParameters,
440 # position = [-1.1,-0.4,2.1],
441 # rotationMatrix = np.eye(3),
442 # angularVelocity = [0,0,0],
443 # gravity = gravity,
444 # graphicsDataList = [GraphicsDataSphere(radius=rr, color=color4green, nTiles=16)])
445 # markerList += [mbs.AddMarker(MarkerNodeRigid(nodeNumber=n0))]
446 # radiusList += [rr]
447
448
449
450 gContact = mbs.AddGeneralContact()
451 gContact.verboseMode = 1
452
453 contactStiffness = 5e5# 1e6*2 #2e6 for test with spheres
454 contactDamping = 0.02*contactStiffness
455
456 for i in range(len(markerList)):
457 m = markerList[i]
458 r = radiusList[i]
459 gContact.AddSphereWithMarker(m, radius=r, contactStiffness=contactStiffness, contactDamping=contactDamping, frictionMaterialIndex=0)
460
461
462 gContact.SetFrictionPairings(0.*np.eye(1))
463 gContact.SetSearchTreeCellSize(numberOfCells=[4,4,4]) #could also be 1,1,1
464 gContact.SetSearchTreeBox(pMin=np.array([-2,-2,-2]), pMax=np.array([2,2,2]))
465
466
467
468
469#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
470#assemble system before solving
471mbs.Assemble()
472
473simulationSettings = exu.SimulationSettings() #takes currently set values or default values
474
475tEnd = 1.25 #simulation time
476h = 0.25*1e-3 #step size
477simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
478simulationSettings.timeIntegration.endTime = tEnd
479simulationSettings.timeIntegration.verboseMode = 1
480#simulationSettings.timeIntegration.simulateInRealtime = True
481simulationSettings.solutionSettings.solutionWritePeriod = 0.005 #store every 5 ms
482
483SC.visualizationSettings.window.renderWindowSize=[1600,1200]
484SC.visualizationSettings.openGL.multiSampling = 4
485SC.visualizationSettings.general.autoFitScene = False
486
487SC.visualizationSettings.nodes.drawNodesAsPoint=False
488SC.visualizationSettings.nodes.showBasis=True
489SC.visualizationSettings.general.drawWorldBasis=True
490SC.visualizationSettings.bodies.kinematicTree.showJointFrames = False
491
492SC.visualizationSettings.openGL.multiSampling=4
493SC.visualizationSettings.openGL.lineWidth = 2
494# SC.visualizationSettings.openGL.shadow=0.3 #don't do this for fine meshes!
495SC.visualizationSettings.openGL.light0position=[-6,2,12,0]
496
497exu.StartRenderer()
498if 'renderState' in exu.sys: #reload old view
499 SC.SetRenderState(exu.sys['renderState'])
500
501mbs.WaitForUserToContinue() #stop before simulating
502
503mbs.SolveDynamic(simulationSettings = simulationSettings,
504 solverType=exu.DynamicSolverType.TrapezoidalIndex2)
505# mbs.SolveDynamic(simulationSettings = simulationSettings,
506# solverType=exu.DynamicSolverType.ExplicitEuler)
507
508# SC.WaitForRenderEngineStopFlag() #stop before closing
509exu.StopRenderer() #safely close rendering window!
510
511sol = LoadSolutionFile('coordinatesSolution.txt')
512
513mbs.SolutionViewer(sol)
514
515if False:
516
517 mbs.PlotSensor([sens1],[1])