diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp
index e3faf48d9..83d262af0 100644
--- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp
+++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp
@@ -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)
{
diff --git a/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf b/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf
index 77a14aeb8..abb3b2edc 100644
--- a/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf
+++ b/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf
@@ -253,6 +253,13 @@
+
+
+
+
+
+
+
@@ -272,6 +279,14 @@
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/examples/testBike.py b/examples/pybullet/gym/pybullet_envs/examples/testBike.py
deleted file mode 100644
index 62475c60d..000000000
--- a/examples/pybullet/gym/pybullet_envs/examples/testBike.py
+++ /dev/null
@@ -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)
diff --git a/examples/pybullet/gym/pybullet_robots/__init__.py b/examples/pybullet/gym/pybullet_robots/__init__.py
new file mode 100644
index 000000000..8b1378917
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/__init__.py
@@ -0,0 +1 @@
+
diff --git a/examples/pybullet/gym/pybullet_envs/examples/laikago.py b/examples/pybullet/gym/pybullet_robots/laikago/laikago.py
similarity index 100%
rename from examples/pybullet/gym/pybullet_envs/examples/laikago.py
rename to examples/pybullet/gym/pybullet_robots/laikago/laikago.py
diff --git a/examples/pybullet/gym/pybullet_robots/panda/__init__.py b/examples/pybullet/gym/pybullet_robots/panda/__init__.py
new file mode 100644
index 000000000..8b1378917
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/__init__.py
@@ -0,0 +1 @@
+
diff --git a/examples/pybullet/gym/pybullet_robots/panda/batchsim3.py b/examples/pybullet/gym/pybullet_robots/panda/batchsim3.py
new file mode 100644
index 000000000..c3d9d65b1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/batchsim3.py
@@ -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()
diff --git a/examples/pybullet/gym/pybullet_robots/panda/batchsim3_grasp.py b/examples/pybullet/gym/pybullet_robots/panda/batchsim3_grasp.py
new file mode 100644
index 000000000..39c8cc41a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/batchsim3_grasp.py
@@ -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()
diff --git a/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py b/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py
new file mode 100644
index 000000000..08057626c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py
@@ -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)
+
diff --git a/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py
new file mode 100644
index 000000000..b8effa89b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py
@@ -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)
+
diff --git a/examples/pybullet/gym/pybullet_robots/panda/panda_sim.py b/examples/pybullet/gym/pybullet_robots/panda/panda_sim.py
new file mode 100644
index 000000000..b4d688687
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/panda_sim.py
@@ -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
+
diff --git a/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py b/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py
new file mode 100644
index 000000000..58721aa6a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py
@@ -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)
diff --git a/examples/pybullet/gym/pybullet_robots/xarm/loadxarm_sim.py b/examples/pybullet/gym/pybullet_robots/xarm/loadxarm_sim.py
new file mode 100644
index 000000000..f25097975
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_robots/xarm/loadxarm_sim.py
@@ -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)
+
diff --git a/examples/pybullet/gym/pybullet_envs/examples/xarm.py b/examples/pybullet/gym/pybullet_robots/xarm/xarm.py
similarity index 100%
rename from examples/pybullet/gym/pybullet_envs/examples/xarm.py
rename to examples/pybullet/gym/pybullet_robots/xarm/xarm.py
diff --git a/examples/pybullet/gym/pybullet_envs/examples/xarm_sim.py b/examples/pybullet/gym/pybullet_robots/xarm/xarm_sim.py
similarity index 100%
rename from examples/pybullet/gym/pybullet_envs/examples/xarm_sim.py
rename to examples/pybullet/gym/pybullet_robots/xarm/xarm_sim.py
diff --git a/setup.py b/setup.py
index bfadab97a..a6caac784 100644
--- a/setup.py
+++ b/setup.py
@@ -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=