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