1import pybullet as p 2import pybullet_data as pd 3import time 4import math 5import pybullet_data 6 7usePhysX = True 8useMaximalCoordinates = True 9if usePhysX: 10 p.connect(p.PhysX, options="--numCores=8 --solver=pgs") 11 p.loadPlugin("eglRendererPlugin") 12else: 13 p.connect(p.GUI) 14 15p.setAdditionalSearchPath(pybullet_data.getDataPath()) 16p.setPhysicsEngineParameter(fixedTimeStep=1. / 240., 17 numSolverIterations=4, 18 minimumSolverIslandSize=1024) 19p.setPhysicsEngineParameter(contactBreakingThreshold=0.01) 20 21p.setAdditionalSearchPath(pd.getDataPath()) 22#Always make ground plane maximal coordinates, to avoid performance drop in PhysX 23#See https://github.com/NVIDIAGameWorks/PhysX/issues/71 24 25p.loadURDF("plane.urdf", useMaximalCoordinates=True) #useMaximalCoordinates) 26p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0) 27p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) 28logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "physx_create_dominoes.json") 29jran = 50 30iran = 100 31 32num = 64 33radius = 0.1 34numDominoes = 0 35 36for i in range(int(num * 50)): 37 num = (radius * 2 * math.pi) / 0.08 38 radius += 0.05 / float(num) 39 orn = p.getQuaternionFromEuler([0, 0, 0.5 * math.pi + math.pi * 2 * i / float(num)]) 40 pos = [ 41 radius * math.cos(2 * math.pi * (i / float(num))), 42 radius * math.sin(2 * math.pi * (i / float(num))), 0.03 43 ] 44 sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates) 45 numDominoes += 1 46 47pos = [pos[0], pos[1], pos[2] + 0.3] 48orn = p.getQuaternionFromEuler([0, 0, -math.pi / 4.]) 49sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates) 50 51print("numDominoes=", numDominoes) 52 53#for j in range (20): 54# for i in range (100): 55# if (i<99): 56# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates) 57# else: 58# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0]) 59# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates) 60 61print("loaded!") 62 63#p.changeDynamics(sphere ,-1, mass=1000) 64 65door = p.loadURDF("door.urdf", [0, 0, -11]) 66p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1) 67print("numJoints = ", p.getNumJoints(door)) 68 69p.setGravity(0, 0, -10) 70position_control = True 71 72angle = math.pi * 0.25 73p.resetJointState(door, 1, angle) 74angleread = p.getJointState(door, 1) 75print("angleread = ", angleread) 76prevTime = time.time() 77 78angle = math.pi * 0.5 79 80count = 0 81while (1): 82 count += 1 83 if (count == 12): 84 p.stopStateLogging(logId) 85 p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) 86 87 curTime = time.time() 88 89 diff = curTime - prevTime 90 #every second, swap target angle 91 if (diff > 1): 92 angle = -angle 93 prevTime = curTime 94 95 if position_control: 96 p.setJointMotorControl2(door, 97 1, 98 p.POSITION_CONTROL, 99 targetPosition=angle, 100 positionGain=10.1, 101 velocityGain=1, 102 force=11.001) 103 else: 104 p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=1, force=1011) 105 #contacts = p.getContactPoints() 106 #print("contacts=",contacts) 107 p.stepSimulation() 108 #time.sleep(1./240.) 109