AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2581 from erwincoumans/master

Browse Source 
bump up pybullet version to 2.6.4
This commit is contained in:
erwincoumans 2020-01-12 08:13:03 -08:00 committed by GitHub
commit 2f09c7c224
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 693 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@ -1106,27 +1106,11 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
int height = (int)m_window->getRetinaScale() * m_instancingRenderer->getScreenHeight();
char cmd[8192];
#ifdef _WIN32
sprintf(cmd,
"ffmpeg -r %d -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b:v 50000k -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s",
m_data->m_mp4Fps, width, height, mp4FileName);
//sprintf(cmd, "ffmpeg -r %d -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", m_data->m_mp4Fps, width, height, mp4FileName);
#else
sprintf(cmd,
"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b 50000k -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s",
width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-threads 0 -y -crf 0 -b 50000k -vf vflip %s",width,height,mp4FileName);
// sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-threads 0 -preset fast -y -crf 21 -vf vflip %s",width,height,mp4FileName);
if (m_data->m_ffmpegFile)
{

15
examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf
View File

@ -253,6 +253,13 @@
</collision>
</link>
<link name="panda_leftfinger">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0.01 0.02"/>
<mass value="0.1"/>
@ -272,6 +279,14 @@
</collision>
</link>
<link name="panda_rightfinger">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
<mass value="0.1"/>

139
examples/pybullet/gym/pybullet_envs/examples/testBike.py
View File

@ -1,139 +0,0 @@
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
import pybullet as p
import math
import time
import pybullet_data
p.connect(p.GUI)
#p.loadURDF("wheel.urdf",[0,0,3])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("plane100.urdf", [0, 0, 0])
timestep = 1. / 240.
bike = -1
for i in range(1):
bike = p.loadURDF("bicycle/bike.urdf", [0, 0 + 3 * i, 1.5], [0, 0, 0, 1], useFixedBase=False)
p.setJointMotorControl2(bike, 0, p.VELOCITY_CONTROL, targetVelocity=0, force=0.05)
#p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000)
p.setJointMotorControl2(bike, 1, p.VELOCITY_CONTROL, targetVelocity=5, force=0)
p.setJointMotorControl2(bike, 2, p.VELOCITY_CONTROL, targetVelocity=15, force=20)
p.changeDynamics(plane, -1, mass=0, lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike, 1, lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike, 2, lateralFriction=1, linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100)
#p.resetJointState(bike,2,0,100)
#p.resetBaseVelocity(bike,[0,0,0],[0,0,0])
#p.setPhysicsEngineParameter(numSubSteps=0)
#bike=p.loadURDF("frame.urdf",useFixedBase=True)
#bike = p.loadURDF("handlebar.urdf", useFixedBase=True)
#p.loadURDF("handlebar.urdf",[0,2,1])
#coord = p.loadURDF("handlebar.urdf", [0.7700000000000005, 0, 0.22000000000000006],useFixedBase=True)# p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]
coordPos = [0.07, 0, -0.6900000000000004]
coordOrnEuler = [0, 0, 0]
coordPos2 = [0, 0, 0]
coordOrnEuler2 = [0, 0, 0]
invPos, invOrn = p.invertTransform(coordPos, p.getQuaternionFromEuler(coordOrnEuler))
mPos, mOrn = p.multiplyTransforms(invPos, invOrn, coordPos2,
p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"", eul[0], eul[1], eul[2], "\" xyz=\"", mPos[0], mPos[1], mPos[2])
shift = 0
gui = 1
frame = 0
states = []
states.append(p.saveState())
#observations=[]
#observations.append(obs)
running = True
reverting = False
p.getCameraImage(320, 200) #,renderer=p.ER_BULLET_HARDWARE_OPENGL )
while (1):
updateCam = 0
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (reverting or (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED))):
reverting = True
stateIndex = len(states) - 1
#print("prestateIndex=",stateIndex)
time.sleep(timestep)
updateCam = 1
if (stateIndex > 0):
stateIndex -= 1
states = states[:stateIndex + 1]
#observations=observations[:stateIndex+1]
#print("states=",states)
#print("stateIndex =",stateIndex )
p.restoreState(states[stateIndex])
#obs=observations[stateIndex]
#obs, r, done, _ = env.step(a)
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
reverting = False
if (k == ord('1') and (v & p.KEY_WAS_TRIGGERED)):
gui = not gui
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
running = False
if (running or (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED))):
running = True
if (running):
p.stepSimulation()
updateCam = 1
time.sleep(timestep)
pts = p.getContactPoints()
#print("numPoints=",len(pts))
#for point in pts:
# print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9])
states.append(p.saveState())
#observations.append(obs)
stateIndex = len(states)
#print("stateIndex =",stateIndex )
frame += 1
if (updateCam):
distance = 5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(bike)
humanBaseVel = p.getBaseVelocity(bike)
#print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel)
if (gui):
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance = camInfo[10]
yaw = camInfo[8]
pitch = camInfo[9]
targetPos = [
0.95 * curTargetPos[0] + 0.05 * humanPos[0], 0.95 * curTargetPos[1] + 0.05 * humanPos[1],
curTargetPos[2]
]
p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)

1
examples/pybullet/gym/pybullet_robots/__init__.py Normal file
View File

@ -0,0 +1 @@

0
examples/pybullet/gym/pybullet_envs/examples/laikago.py → examples/pybullet/gym/pybullet_robots/laikago/laikago.py
View File

1
examples/pybullet/gym/pybullet_robots/panda/__init__.py Normal file
View File

@ -0,0 +1 @@

189
examples/pybullet/gym/pybullet_robots/panda/batchsim3.py Normal file
View File

@ -0,0 +1,189 @@
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
from pybullet_utils import bullet_client
import panda_sim
import time
useGUI = False
timeStep = 1./60.
# Importing the libraries
import os
import time
import multiprocessing as mp
from multiprocessing import Process, Pipe
pandaEndEffectorIndex = 11 #8
pandaNumDofs = 7
_RESET = 1
_CLOSE = 2
_EXPLORE = 3
def ExploreWorker(rank, num_processes, childPipe, args):
print("hi:",rank, " out of ", num_processes)
import pybullet as op1
import pybullet_data as pd
logName=""
p1=0
n = 0
space = 2
simulations=[]
sims_per_worker = 10
offsetY = rank*space
while True:
n += 1
try:
# Only block for short times to have keyboard exceptions be raised.
if not childPipe.poll(0.0001):
continue
message, payload = childPipe.recv()
except (EOFError, KeyboardInterrupt):
break
if message == _RESET:
if (useGUI):
p1 = bullet_client.BulletClient(op1.GUI)
else:
p1 = bullet_client.BulletClient(op1.DIRECT)
p1.setTimeStep(timeStep)
p1.setPhysicsEngineParameter(numSolverIterations=8)
p1.setPhysicsEngineParameter(minimumSolverIslandSize=100)
p1.configureDebugVisualizer(p1.COV_ENABLE_Y_AXIS_UP,1)
p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING,0)
p1.setAdditionalSearchPath(pd.getDataPath())
p1.setGravity(0,-9.8,0)
logName = str("batchsim")+str(rank)
for j in range (3):
offsetX = 0#-sims_per_worker/2.0*space
for i in range (sims_per_worker):
offset=[offsetX,0, offsetY]
sim = panda_sim.PandaSim(p1, offset)
simulations.append(sim)
offsetX += space
offsetY += space
childPipe.send(["reset ok"])
p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING,1)
for i in range (100):
p1.stepSimulation()
logId = p1.startStateLogging(op1.STATE_LOGGING_PROFILE_TIMINGS,logName)
continue
if message == _EXPLORE:
sum_rewards=rank
if useGUI:
numSteps = int(20000)
else:
numSteps = int(5)
for i in range (numSteps):
for s in simulations:
s.step()
p1.stepSimulation()
#print("logId=",logId)
#print("numSteps=",numSteps)
childPipe.send([sum_rewards])
continue
if message == _CLOSE:
p1.stopStateLogging(logId)
childPipe.send(["close ok"])
break
childPipe.close()
if __name__ == "__main__":
mp.freeze_support()
if useGUI:
num_processes = 1
else:
num_processes = 12
processes = []
args=[0]*num_processes
childPipes = []
parentPipes = []
for pr in range(num_processes):
parentPipe, childPipe = Pipe()
parentPipes.append(parentPipe)
childPipes.append(childPipe)
for rank in range(num_processes):
p = mp.Process(target=ExploreWorker, args=(rank, num_processes, childPipes[rank], args))
p.start()
processes.append(p)
for parentPipe in parentPipes:
parentPipe.send([_RESET, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
#print("reset msg=",parentPipes[k].recv()[0])
msg = parentPipes[k].recv()[0]
for parentPipe in parentPipes:
parentPipe.send([_EXPLORE, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
positive_rewards[k] = parentPipes[k].recv()[0]
#print("positive_rewards=",positive_rewards[k])
for parentPipe in parentPipes:
parentPipe.send([_EXPLORE, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
positive_rewards[k] = parentPipes[k].recv()[0]
#print("positive_rewards=",positive_rewards[k])
msg = positive_rewards[k]
for parentPipe in parentPipes:
parentPipe.send([_EXPLORE, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
positive_rewards[k] = parentPipes[k].recv()[0]
#print("positive_rewards=",positive_rewards[k])
for parentPipe in parentPipes:
parentPipe.send([_CLOSE, "pay2"])
for p in processes:
p.join()
#now we merge the separate json files into a single one
fnameout = 'batchsim.json'
count = 0
outfile = open(fnameout, "w+")
outfile.writelines(["{\"traceEvents\":[\n"])
numFiles = num_processes
for num in range(numFiles):
print("num=",num)
fname = 'batchsim%d_0.json' % (num)
with open(fname) as infile:
for line in infile:
if "pid" in line:
line = line.replace('\"pid\":1', '\"pid\":'+str(num))
if num < (numFiles-1) and not "{}}," in line:
line = line.replace('{}}', '{}},')
print("line[",count,"]=",line)
outfile.write(line)
count += 1
print ("count=",count)
outfile.writelines(["],\n"])
outfile.writelines(["\"displayTimeUnit\": \"ns\"}\n"])
outfile.close()

186
examples/pybullet/gym/pybullet_robots/panda/batchsim3_grasp.py Normal file
View File

@ -0,0 +1,186 @@
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
from pybullet_utils import bullet_client
import panda_sim_grasp as panda_sim
import time
useGUI = True#False
timeStep = 1./240.
# Importing the libraries
import os
import time
import multiprocessing as mp
from multiprocessing import Process, Pipe
pandaEndEffectorIndex = 11 #8
pandaNumDofs = 7
_RESET = 1
_CLOSE = 2
_EXPLORE = 3
def ExploreWorker(rank, num_processes, childPipe, args):
print("hi:",rank, " out of ", num_processes)
import pybullet as op1
import pybullet_data as pd
logName=""
p1=0
n = 0
space = 2
simulations=[]
sims_per_worker = 10
offsetY = rank*space
while True:
n += 1
try:
# Only block for short times to have keyboard exceptions be raised.
if not childPipe.poll(0.0001):
continue
message, payload = childPipe.recv()
except (EOFError, KeyboardInterrupt):
break
if message == _RESET:
if (useGUI):
p1 = bullet_client.BulletClient(op1.GUI)
else:
p1 = bullet_client.BulletClient(op1.DIRECT)
p1.setTimeStep(timeStep)
p1.setPhysicsEngineParameter(numSolverIterations=8)
p1.setPhysicsEngineParameter(minimumSolverIslandSize=100)
p1.configureDebugVisualizer(p1.COV_ENABLE_Y_AXIS_UP,1)
p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING,0)
p1.setAdditionalSearchPath(pd.getDataPath())
p1.setGravity(0,-9.8,0)
logName = str("batchsim")+str(rank)
for j in range (3):
offsetX = 0#-sims_per_worker/2.0*space
for i in range (sims_per_worker):
offset=[offsetX,0, offsetY]
sim = panda_sim.PandaSimAuto(p1, offset)
simulations.append(sim)
offsetX += space
offsetY += space
childPipe.send(["reset ok"])
p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING,1)
for i in range (100):
p1.stepSimulation()
logId = p1.startStateLogging(op1.STATE_LOGGING_PROFILE_TIMINGS,logName)
continue
if message == _EXPLORE:
sum_rewards=rank
if useGUI:
numSteps = int(20000)
else:
numSteps = int(5)
for i in range (numSteps):
for s in simulations:
s.step()
p1.stepSimulation()
#print("logId=",logId)
#print("numSteps=",numSteps)
childPipe.send([sum_rewards])
continue
if message == _CLOSE:
p1.stopStateLogging(logId)
childPipe.send(["close ok"])
break
childPipe.close()
if __name__ == "__main__":
mp.freeze_support()
if useGUI:
num_processes = 1
else:
num_processes = 12
processes = []
args=[0]*num_processes
childPipes = []
parentPipes = []
for pr in range(num_processes):
parentPipe, childPipe = Pipe()
parentPipes.append(parentPipe)
childPipes.append(childPipe)
for rank in range(num_processes):
p = mp.Process(target=ExploreWorker, args=(rank, num_processes, childPipes[rank], args))
p.start()
processes.append(p)
for parentPipe in parentPipes:
parentPipe.send([_RESET, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
#print("reset msg=",parentPipes[k].recv()[0])
msg = parentPipes[k].recv()[0]
for parentPipe in parentPipes:
parentPipe.send([_EXPLORE, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
positive_rewards[k] = parentPipes[k].recv()[0]
#print("positive_rewards=",positive_rewards[k])
for parentPipe in parentPipes:
parentPipe.send([_EXPLORE, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
positive_rewards[k] = parentPipes[k].recv()[0]
#print("positive_rewards=",positive_rewards[k])
msg = positive_rewards[k]
for parentPipe in parentPipes:
parentPipe.send([_EXPLORE, "blaat"])
positive_rewards = [0]*num_processes
for k in range(num_processes):
positive_rewards[k] = parentPipes[k].recv()[0]
#print("positive_rewards=",positive_rewards[k])
for parentPipe in parentPipes:
parentPipe.send([_CLOSE, "pay2"])
for p in processes:
p.join()
#now we merge the separate json files into a single one
fnameout = 'batchsim.json'
count = 0
outfile = open(fnameout, "w+")
outfile.writelines(["{\"traceEvents\":[\n"])
numFiles = num_processes
for num in range(numFiles):
print("num=",num)
fname = 'batchsim%d_0.json' % (num)
with open(fname) as infile:
for line in infile:
if "pid" in line:
line = line.replace('\"pid\":1', '\"pid\":'+str(num))
if num < (numFiles-1) and not "{}}," in line:
line = line.replace('{}}', '{}},')
print("line[",count,"]=",line)
outfile.write(line)
count += 1
print ("count=",count)
outfile.writelines(["],\n"])
outfile.writelines(["\"displayTimeUnit\": \"ns\"}\n"])
outfile.close()

21
examples/pybullet/gym/pybullet_robots/panda/loadpanda.py Normal file
View File

@ -0,0 +1,21 @@
import pybullet as p
import pybullet_data as pd
import math
import time
import numpy as np
import pybullet_robots.panda.panda_sim as panda_sim
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1)
p.setAdditionalSearchPath(pd.getDataPath())
timeStep=1./60.
p.setTimeStep(timeStep)
p.setGravity(0,-9.8,0)
panda = panda_sim.PandaSim(p,[0,0,0])
while (1):
panda.step()
p.stepSimulation()
time.sleep(timeStep)

43
examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py Normal file
View File

@ -0,0 +1,43 @@
import pybullet as p
import pybullet_data as pd
import math
import time
import numpy as np
import pybullet_robots.panda.panda_sim_grasp as panda_sim
#video requires ffmpeg available in path
createVideo=False
fps=240.
timeStep = 1./fps
if createVideo:
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=0 --mp4=\"pybullet_grasp.mp4\" --mp4fps="+str(fps) )
else:
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000)
p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0])
p.setAdditionalSearchPath(pd.getDataPath())
p.setTimeStep(timeStep)
p.setGravity(0,-9.8,0)
panda = panda_sim.PandaSimAuto(p,[0,0,0])
panda.control_dt = timeStep
logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
panda.bullet_client.submitProfileTiming("start")
for i in range (100000):
panda.bullet_client.submitProfileTiming("full_step")
panda.step()
p.stepSimulation()
if createVideo:
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
if not createVideo:
time.sleep(timeStep)
panda.bullet_client.submitProfileTiming()
panda.bullet_client.submitProfileTiming()
panda.bullet_client.stopStateLogging(logId)

65
examples/pybullet/gym/pybullet_robots/panda/panda_sim.py Normal file
View File

@ -0,0 +1,65 @@
import time
import numpy as np
import math
useNullSpace = 1
ikSolver = 0
pandaEndEffectorIndex = 11 #8
pandaNumDofs = 7
ll = [-7]*pandaNumDofs
#upper limits for null space (todo: set them to proper range)
ul = [7]*pandaNumDofs
#joint ranges for null space (todo: set them to proper range)
jr = [7]*pandaNumDofs
#restposes for null space
jointPositions=[0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02]
rp = jointPositions
class PandaSim(object):
def __init__(self, bullet_client, offset):
self.bullet_client = bullet_client
self.offset = np.array(offset)
#print("offset=",offset)
flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES
legos=[]
self.bullet_client.loadURDF("tray/traybox.urdf", [0+offset[0], 0+offset[1], -0.6+offset[2]], [-0.5, -0.5, -0.5, 0.5], flags=flags)
legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.5])+self.offset, flags=flags))
legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([-0.1, 0.3, -0.5])+self.offset, flags=flags))
legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.7])+self.offset, flags=flags))
sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.6])+self.offset, flags=flags)
self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.5])+self.offset, flags=flags)
self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.7])+self.offset, flags=flags)
orn=[-0.707107, 0.0, 0.0, 0.707107]#p.getQuaternionFromEuler([-math.pi/2,math.pi/2,0])
eul = self.bullet_client.getEulerFromQuaternion([-0.5, -0.5, -0.5, 0.5])
self.panda = self.bullet_client.loadURDF("franka_panda/panda.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags)
index = 0
for j in range(self.bullet_client.getNumJoints(self.panda)):
self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0)
info = self.bullet_client.getJointInfo(self.panda, j)
jointName = info[1]
jointType = info[2]
if (jointType == self.bullet_client.JOINT_PRISMATIC):
self.bullet_client.resetJointState(self.panda, j, jointPositions[index])
index=index+1
if (jointType == self.bullet_client.JOINT_REVOLUTE):
self.bullet_client.resetJointState(self.panda, j, jointPositions[index])
index=index+1
self.t = 0.
def reset(self):
pass
def step(self):
t = self.t
self.t += 1./60.
pos = [self.offset[0]+0.2 * math.sin(1.5 * t), self.offset[1]+0.044, self.offset[2]+-0.6 + 0.1 * math.cos(1.5 * t)]
orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.])
jointPoses = self.bullet_client.calculateInverseKinematics(self.panda,pandaEndEffectorIndex, pos, orn, ll, ul,
jr, rp, maxNumIterations=5)
for i in range(pandaNumDofs):
self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.)
pass

151
examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py Normal file
View File

@ -0,0 +1,151 @@
import time
import numpy as np
import math
useNullSpace = 1
ikSolver = 0
pandaEndEffectorIndex = 11 #8
pandaNumDofs = 7
ll = [-7]*pandaNumDofs
#upper limits for null space (todo: set them to proper range)
ul = [7]*pandaNumDofs
#joint ranges for null space (todo: set them to proper range)
jr = [7]*pandaNumDofs
#restposes for null space
jointPositions=[0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02]
rp = jointPositions
class PandaSim(object):
def __init__(self, bullet_client, offset):
self.bullet_client = bullet_client
self.bullet_client.setPhysicsEngineParameter(solverResidualThreshold=0)
self.offset = np.array(offset)
#print("offset=",offset)
flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES
self.legos=[]
self.bullet_client.loadURDF("tray/traybox.urdf", [0+offset[0], 0+offset[1], -0.6+offset[2]], [-0.5, -0.5, -0.5, 0.5], flags=flags)
self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.5])+self.offset, flags=flags))
self.bullet_client.changeVisualShape(self.legos[0],-1,rgbaColor=[1,0,0,1])
self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([-0.1, 0.3, -0.5])+self.offset, flags=flags))
self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.7])+self.offset, flags=flags))
self.sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.6])+self.offset, flags=flags)
self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.5])+self.offset, flags=flags)
self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.7])+self.offset, flags=flags)
orn=[-0.707107, 0.0, 0.0, 0.707107]#p.getQuaternionFromEuler([-math.pi/2,math.pi/2,0])
eul = self.bullet_client.getEulerFromQuaternion([-0.5, -0.5, -0.5, 0.5])
self.panda = self.bullet_client.loadURDF("franka_panda/panda.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags)
index = 0
self.state = 0
self.control_dt = 1./240.
self.finger_target = 0
self.gripper_height = 0.2
#create a constraint to keep the fingers centered
c = self.bullet_client.createConstraint(self.panda,
9,
self.panda,
10,
jointType=self.bullet_client.JOINT_GEAR,
jointAxis=[1, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self.bullet_client.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50)
for j in range(self.bullet_client.getNumJoints(self.panda)):
self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0)
info = self.bullet_client.getJointInfo(self.panda, j)
#print("info=",info)
jointName = info[1]
jointType = info[2]
if (jointType == self.bullet_client.JOINT_PRISMATIC):
self.bullet_client.resetJointState(self.panda, j, jointPositions[index])
index=index+1
if (jointType == self.bullet_client.JOINT_REVOLUTE):
self.bullet_client.resetJointState(self.panda, j, jointPositions[index])
index=index+1
self.t = 0.
def reset(self):
pass
def update_state(self):
keys = self.bullet_client.getKeyboardEvents()
if len(keys)>0:
for k,v in keys.items():
if v&self.bullet_client.KEY_WAS_TRIGGERED:
if (k==ord('1')):
self.state = 1
if (k==ord('2')):
self.state = 2
if (k==ord('3')):
self.state = 3
if (k==ord('4')):
self.state = 4
if (k==ord('5')):
self.state = 5
if (k==ord('6')):
self.state = 6
if v&self.bullet_client.KEY_WAS_RELEASED:
self.state = 0
def step(self):
if self.state==6:
self.finger_target = 0.01
if self.state==5:
self.finger_target = 0.04
self.bullet_client.submitProfileTiming("step")
self.update_state()
#print("self.state=",self.state)
#print("self.finger_target=",self.finger_target)
alpha = 0.9 #0.99
if self.state==1 or self.state==2 or self.state==3 or self.state==4 or self.state==7:
#gripper_height = 0.034
self.gripper_height = alpha * self.gripper_height + (1.-alpha)*0.03
if self.state == 2 or self.state == 3 or self.state == 7:
self.gripper_height = alpha * self.gripper_height + (1.-alpha)*0.2
t = self.t
self.t += self.control_dt
pos = [self.offset[0]+0.2 * math.sin(1.5 * t), self.offset[1]+self.gripper_height, self.offset[2]+-0.6 + 0.1 * math.cos(1.5 * t)]
if self.state == 3 or self.state== 4:
pos, o = self.bullet_client.getBasePositionAndOrientation(self.legos[0])
pos = [pos[0], self.gripper_height, pos[2]]
self.prev_pos = pos
if self.state == 7:
pos = self.prev_pos
diffX = pos[0] - self.offset[0]
diffZ = pos[2] - (self.offset[2]-0.6)
self.prev_pos = [self.prev_pos[0] - diffX*0.1, self.prev_pos[1], self.prev_pos[2]-diffZ*0.1]
orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.])
self.bullet_client.submitProfileTiming("IK")
jointPoses = self.bullet_client.calculateInverseKinematics(self.panda,pandaEndEffectorIndex, pos, orn, ll, ul,
jr, rp, maxNumIterations=20)
self.bullet_client.submitProfileTiming()
for i in range(pandaNumDofs):
self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.)
#target for fingers
for i in [9,10]:
self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL,self.finger_target ,force= 10)
self.bullet_client.submitProfileTiming()
class PandaSimAuto(PandaSim):
def __init__(self, bullet_client, offset):
PandaSim.__init__(self, bullet_client, offset)
self.state_t = 0
self.cur_state = 0
self.states=[0,3,5,4,6,3,7]
self.state_durations=[1,1,1,2,1,1, 10]
def update_state(self):
self.state_t += self.control_dt
if self.state_t > self.state_durations[self.cur_state]:
self.cur_state += 1
if self.cur_state>=len(self.states):
self.cur_state = 0
self.state_t = 0
self.state=self.states[self.cur_state]
#print("self.state=",self.state)

20
examples/pybullet/gym/pybullet_robots/xarm/loadxarm_sim.py Normal file
View File

@ -0,0 +1,20 @@
import pybullet as p
import pybullet_data as pd
import math
import time
import numpy as np
import pybullet_robots.xarm.xarm_sim as xarm_sim
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
timeStep=1./60.
p.setTimeStep(timeStep)
p.setGravity(0,0,-9.8)
xarm = xarm_sim.XArm6Sim(p,[0,0,0])
while (1):
xarm.step()
p.stepSimulation()
time.sleep(timeStep)

0
examples/pybullet/gym/pybullet_envs/examples/xarm.py → examples/pybullet/gym/pybullet_robots/xarm/xarm.py
View File

0
examples/pybullet/gym/pybullet_envs/examples/xarm_sim.py → examples/pybullet/gym/pybullet_robots/xarm/xarm_sim.py
View File

2
setup.py
View File

@ -491,7 +491,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name='pybullet',
version='2.6.3',
version='2.6.4',
description=
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description=