107 lines
3.2 KiB
Python
107 lines
3.2 KiB
Python
import pybullet as p
|
|
import pybullet_data as pd
|
|
import time
|
|
import math
|
|
|
|
usePhysX = True
|
|
useMaximalCoordinates = True
|
|
if usePhysX:
|
|
p.connect(p.PhysX, options="--numCores=8 --solver=pgs")
|
|
p.loadPlugin("eglRendererPlugin")
|
|
else:
|
|
p.connect(p.GUI)
|
|
|
|
p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.,
|
|
numSolverIterations=4,
|
|
minimumSolverIslandSize=1024)
|
|
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
|
|
|
|
p.setAdditionalSearchPath(pd.getDataPath())
|
|
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
|
|
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
|
|
|
|
p.loadURDF("plane.urdf", useMaximalCoordinates=True) #useMaximalCoordinates)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
|
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "physx_create_dominoes.json")
|
|
jran = 50
|
|
iran = 100
|
|
|
|
num = 64
|
|
radius = 0.1
|
|
numDominoes = 0
|
|
|
|
for i in range(int(num * 50)):
|
|
num = (radius * 2 * math.pi) / 0.08
|
|
radius += 0.05 / float(num)
|
|
orn = p.getQuaternionFromEuler([0, 0, 0.5 * math.pi + math.pi * 2 * i / float(num)])
|
|
pos = [
|
|
radius * math.cos(2 * math.pi * (i / float(num))),
|
|
radius * math.sin(2 * math.pi * (i / float(num))), 0.03
|
|
]
|
|
sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
|
|
numDominoes += 1
|
|
|
|
pos = [pos[0], pos[1], pos[2] + 0.3]
|
|
orn = p.getQuaternionFromEuler([0, 0, -math.pi / 4.])
|
|
sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
|
|
|
|
print("numDominoes=", numDominoes)
|
|
|
|
#for j in range (20):
|
|
# for i in range (100):
|
|
# if (i<99):
|
|
# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
|
|
# else:
|
|
# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
|
|
# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
|
|
|
|
print("loaded!")
|
|
|
|
#p.changeDynamics(sphere ,-1, mass=1000)
|
|
|
|
door = p.loadURDF("door.urdf", [0, 0, -11])
|
|
p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
|
|
print("numJoints = ", p.getNumJoints(door))
|
|
|
|
p.setGravity(0, 0, -10)
|
|
position_control = True
|
|
|
|
angle = math.pi * 0.25
|
|
p.resetJointState(door, 1, angle)
|
|
angleread = p.getJointState(door, 1)
|
|
print("angleread = ", angleread)
|
|
prevTime = time.time()
|
|
|
|
angle = math.pi * 0.5
|
|
|
|
count = 0
|
|
while (1):
|
|
count += 1
|
|
if (count == 12):
|
|
p.stopStateLogging(logId)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
|
|
|
curTime = time.time()
|
|
|
|
diff = curTime - prevTime
|
|
#every second, swap target angle
|
|
if (diff > 1):
|
|
angle = -angle
|
|
prevTime = curTime
|
|
|
|
if position_control:
|
|
p.setJointMotorControl2(door,
|
|
1,
|
|
p.POSITION_CONTROL,
|
|
targetPosition=angle,
|
|
positionGain=10.1,
|
|
velocityGain=1,
|
|
force=11.001)
|
|
else:
|
|
p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
|
|
#contacts = p.getContactPoints()
|
|
#print("contacts=",contacts)
|
|
p.stepSimulation()
|
|
#time.sleep(1./240.)
|