#!/usr/bin/python #\file manyspheres.py #\brief certain python script #\author Akihiko Yamaguchi, [email protected] #\version 0.1 #\date Sep.15, 2020 import pybullet as p import time import pybullet_data if __name__ == '__main__': p.connect(p.GUI, options="--opengl2") #p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setInternalSimFlags(0) p.resetSimulation() p.loadURDF("plane.urdf", useMaximalCoordinates=True) #p.loadURDF("tray/traybox.urdf", useMaximalCoordinates=True) #p.loadURDF("tray_test.urdf", useMaximalCoordinates=True) p.loadURDF("tray_test2.urdf", useMaximalCoordinates=True) t0 = time.clock() p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 1) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
import pybullet as p
CONTROLLER_ID = 0
POSITION=1
ORIENTATION=2
NUM_MOVE_EVENTS=5
BUTTONS=6
ANALOG_AXIS=8
#assume that the VR physics server is already started before
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS
colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
widths = [3]*p.VR_MAX_CONTROLLERS
#use a few default colors
colors[0] = [0,0,0]
colors[1] = [0.5,0,0]
colors[2] = [0,0.5,0]
colors[3] = [0,0,0.5]
colors[4] = [0.5,0.5,0.]
colors[5] = [.5,.5,.5]
POSITION = 1
ORIENTATION = 2
NUM_MOVE_EVENTS = 5
BUTTONS = 6
ANALOG_AXIS = 8
#assume that the VR physics server is already started before
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setInternalSimFlags(0) #don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
prevPosition = [[0, 0, 0]] * p.VR_MAX_CONTROLLERS
colors = [0., 0.5, 0.5] * p.VR_MAX_CONTROLLERS
widths = [3] * p.VR_MAX_CONTROLLERS
#use a few default colors
colors[0] = [0, 0, 0]
colors[1] = [0.5, 0, 0]
colors[2] = [0, 0.5, 0]
colors[3] = [0, 0, 0.5]
colors[4] = [0.5, 0.5, 0.]
colors[5] = [.5, .5, .5]
import pybullet as p
import time
conid = p.connect(p.SHARED_MEMORY)
if (conid<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)
p.resetSimulation()
p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
for i in range (10):
for j in range (10):
for k in range (10):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while True:
gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid)
def main():
import time
from threading import Thread
conid = p.connect(p.SHARED_MEMORY)
if (conid<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)
p.resetSimulation()
sphereRadius = 0.03
gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,5)
rotBase = p.addUserDebugParameter("rotateBase",-3,3,0)
movBase = p.addUserDebugParameter("moveBase",-3,3,0)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
# p.setRealTimeSimulation(1)
distance = 0.1
centers = [(distance * i , 0 ,0) for i in range(5)]
tentacles = [Tentacle(7, c, sphereRadius) for c in centers]
controller = TentacleController()
for tentacle in tentacles:
controller.add_tentacle(tentacle)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
play = True
def update_physics():
stop = time.time()
t = 1/240
p.setGravity(0,0,1)
while True:
if not play:
return
t = (t + time.time() - stop) / 2
p.setTimeStep(t)
p.stepSimulation()
stop = time.time()
time.sleep(0.01)
t = Thread(target=update_physics, args=())
t.daemon = True
t.start()
started = False
while True:
if not started:
start_time = time.time()
started = True
# Move tentacles to match orientation of planes
for tentacle, c in zip(tentacles, centers):
tentacle.move([c[0],c[1],p.readUserDebugParameter(movBase)], p.getQuaternionFromEuler([p.readUserDebugParameter(rotBase),0,0]))
controller.update()
# p.applyExternalForce(tentacle.particles[-1], -1, [0,0,-50], pos, p.WORLD_FRAME)
gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid)
gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX,gravY,gravZ)
time.sleep(0.01)
play = False
def testSaveRestoreState(self):
numSteps = 500
numSteps2 = 30
verbose = 0
self.setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
self.dumpStateToFile(file)
file.close()
#################################
self.setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
stateId = p.saveState()
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
self.dumpStateToFile(file)
file.close()
p.restoreState(stateId)
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
self.dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
def testSaveRestoreState(self):
numSteps = 500
numSteps2 = 30
verbose = 0
self.setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
self.dumpStateToFile(file)
file.close()
#################################
self.setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
stateId = p.saveState()
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
self.dumpStateToFile(file)
file.close()
p.restoreState(stateId)
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
self.dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
