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