From 3f11b03255525d56627260396bd5338638359f43 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 11 Jan 2020 13:13:28 -0800 Subject: [PATCH 1/6] add friction anchors for Panda gripper (prevents/reduces sliding objects out of gripper) --- .../gym/pybullet_data/franka_panda/panda.urdf | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 @@ + + + + + + + + From e73de6ea00c4fc8f4717bee0dc705799e3ce27ff Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 11 Jan 2020 16:06:14 -0800 Subject: [PATCH 2/6] bump up pybullet version to 2.6.4 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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= From 3ca233193f6fa5a674b7acfad8995e5705b91122 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 12 Jan 2020 07:03:38 -0800 Subject: [PATCH 3/6] : --- .../pybullet/gym/pybullet_robots/__init__.py | 1 + .../gym/pybullet_robots/panda/__init__.py | 1 + .../gym/pybullet_robots/panda/batchsim3.py | 189 ++++++++++++++++++ .../pybullet_robots/panda/batchsim3_grasp.py | 186 +++++++++++++++++ .../gym/pybullet_robots/panda/loadpanda.py | 21 ++ .../pybullet_robots/panda/loadpanda_grasp.py | 39 ++++ .../gym/pybullet_robots/panda/panda_sim.py | 65 ++++++ .../pybullet_robots/panda/panda_sim_grasp.py | 151 ++++++++++++++ 8 files changed, 653 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_robots/__init__.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/__init__.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/batchsim3.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/batchsim3_grasp.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/loadpanda.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/panda_sim.py create mode 100644 examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py 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_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..207a5ba40 --- /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 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) + \ No newline at end of file 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..d6fd15687 --- /dev/null +++ b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py @@ -0,0 +1,39 @@ +import pybullet as p +import pybullet_data as pd +import math +import time +import numpy as np +import panda_sim_grasp as panda_sim + +#video requires ffmpeg available in path +createVideo=False + +if createVideo: + p.connect(p.GUI, options="--mp4=\"pybullet_grasp.mp4\", --mp4fps=240") +else: + p.connect(p.GUI) + +p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + +p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0]) +p.setAdditionalSearchPath(pd.getDataPath()) + +timeStep=1./120.#240. +p.setTimeStep(timeStep) +p.setGravity(0,-9.8,0) + +panda = panda_sim.PandaSimAuto(p,[0,0,0]) +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) + 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..05b561050 --- /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./120. + 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) From fb15aea414c1de4c214faca11e0172521dafd904 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 12 Jan 2020 07:11:57 -0800 Subject: [PATCH 4/6] add example robots in pybullet_robots module --- .../gym/pybullet_envs/examples/testBike.py | 139 ------------------ .../laikago}/laikago.py | 0 .../examples => pybullet_robots/xarm}/xarm.py | 0 .../xarm}/xarm_sim.py | 0 4 files changed, 139 deletions(-) delete mode 100644 examples/pybullet/gym/pybullet_envs/examples/testBike.py rename examples/pybullet/gym/{pybullet_envs/examples => pybullet_robots/laikago}/laikago.py (100%) rename examples/pybullet/gym/{pybullet_envs/examples => pybullet_robots/xarm}/xarm.py (100%) rename examples/pybullet/gym/{pybullet_envs/examples => pybullet_robots/xarm}/xarm_sim.py (100%) 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_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_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 From 84e2ea918c2317d43f738203e56f61310f40e36e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 12 Jan 2020 07:20:32 -0800 Subject: [PATCH 5/6] add xarm, modify loadpanda to work with pybullet_robots module --- .../gym/pybullet_robots/panda/loadpanda.py | 4 ++-- .../pybullet_robots/panda/loadpanda_grasp.py | 2 +- .../gym/pybullet_robots/xarm/loadxarm_sim.py | 20 +++++++++++++++++++ 3 files changed, 23 insertions(+), 3 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_robots/xarm/loadxarm_sim.py diff --git a/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py b/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py index 207a5ba40..08057626c 100644 --- a/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py +++ b/examples/pybullet/gym/pybullet_robots/panda/loadpanda.py @@ -3,7 +3,7 @@ import pybullet_data as pd import math import time import numpy as np -import panda_sim +import pybullet_robots.panda.panda_sim as panda_sim p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1) @@ -18,4 +18,4 @@ while (1): panda.step() p.stepSimulation() time.sleep(timeStep) - \ No newline at end of file + diff --git a/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py index d6fd15687..b31b39a8f 100644 --- a/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py +++ b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py @@ -3,7 +3,7 @@ import pybullet_data as pd import math import time import numpy as np -import panda_sim_grasp as panda_sim +import pybullet_robots.panda.panda_sim_grasp as panda_sim #video requires ffmpeg available in path createVideo=False 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) + From 1be6a1d16b843b595bb8bbad583bbb989e6fcf9a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 12 Jan 2020 08:07:54 -0800 Subject: [PATCH 6/6] fix loadpanda video generation remove duplicate code (formerly Windows ffpmeg needed different settings?) --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 16 ---------------- .../gym/pybullet_robots/panda/loadpanda_grasp.py | 12 ++++++++---- .../gym/pybullet_robots/panda/panda_sim_grasp.py | 2 +- 3 files changed, 9 insertions(+), 21 deletions(-) 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_robots/panda/loadpanda_grasp.py b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py index b31b39a8f..b8effa89b 100644 --- a/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py +++ b/examples/pybullet/gym/pybullet_robots/panda/loadpanda_grasp.py @@ -7,23 +7,26 @@ 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="--mp4=\"pybullet_grasp.mp4\", --mp4fps=240") + 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()) -timeStep=1./120.#240. 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): @@ -32,7 +35,8 @@ for i in range (100000): p.stepSimulation() if createVideo: p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1) - time.sleep(timeStep) + 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_grasp.py b/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py index 05b561050..58721aa6a 100644 --- a/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py +++ b/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py @@ -39,7 +39,7 @@ class PandaSim(object): 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./120. + self.control_dt = 1./240. self.finger_target = 0 self.gripper_height = 0.2 #create a constraint to keep the fingers centered