From 0abe4151e5cda62dbe92416c0a3470d283f72a90 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 18 May 2018 16:23:54 -0700 Subject: [PATCH] Fix for 1643, allow to instantiate multiple PyBullet Gym environments (Ant, Humanoid, Hopper, Pendula etc) in the same process (same or other thread). It uses the pybullet_utils.bullet_client to achieve this. --- .../gym/pybullet_data/policies/__init__.py | 0 .../pybullet_data/policies/ppo/__init__.py | 0 .../ppo/minitaur_reactive_env/__init__.py | 0 .../ppo/minitaur_trotting_env/__init__.py | 0 .../pybullet/gym/pybullet_envs/Trainer.py | 111 ----------------- .../pybullet/gym/pybullet_envs/env_bases.py | 44 ++++--- .../enjoy_TF_AntBulletEnv_v0_2017may.py | 18 +-- ...njoy_TF_HalfCheetahBulletEnv_v0_2017may.py | 20 --- .../enjoy_TF_HopperBulletEnv_v0_2017may.py | 19 --- .../enjoy_TF_HumanoidBulletEnv_v0_2017may.py | 18 --- ...manoidFlagrunHarderBulletEnv_v1_2017jul.py | 15 --- ...ertedDoublePendulumBulletEnv_v0_2017may.py | 1 - ...TF_InvertedPendulumBulletEnv_v0_2017may.py | 1 - ...rtedPendulumSwingupBulletEnv_v0_2017may.py | 1 - .../enjoy_TF_Walker2DBulletEnv_v0_2017may.py | 22 +--- .../examples/simpleHumanoidGymEnvTest.py | 31 ----- .../gym/pybullet_envs/gym_locomotion_envs.py | 24 ++-- .../gym/pybullet_envs/gym_manipulator_envs.py | 16 +-- .../gym/pybullet_envs/gym_pendulum_envs.py | 14 +-- .../pybullet/gym/pybullet_envs/robot_bases.py | 115 ++++++++++-------- .../gym/pybullet_envs/robot_locomotors.py | 54 ++++---- .../gym/pybullet_envs/robot_pendula.py | 6 +- .../gym/pybullet_envs/scene_abstract.py | 18 +-- .../gym/pybullet_envs/scene_stadium.py | 18 +-- 24 files changed, 163 insertions(+), 403 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_data/policies/__init__.py create mode 100644 examples/pybullet/gym/pybullet_data/policies/ppo/__init__.py create mode 100644 examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/__init__.py create mode 100644 examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/__init__.py delete mode 100644 examples/pybullet/gym/pybullet_envs/Trainer.py delete mode 100644 examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py diff --git a/examples/pybullet/gym/pybullet_data/policies/__init__.py b/examples/pybullet/gym/pybullet_data/policies/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/pybullet_data/policies/ppo/__init__.py b/examples/pybullet/gym/pybullet_data/policies/ppo/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/__init__.py b/examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/__init__.py b/examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/pybullet_envs/Trainer.py b/examples/pybullet/gym/pybullet_envs/Trainer.py deleted file mode 100644 index 705d3f61a..000000000 --- a/examples/pybullet/gym/pybullet_envs/Trainer.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python - -import argparse # parse input arguments -import numpy as np # arithmetic library -import time -import gym -from agents import agent_register -import pybullet as p -import kerasrl_utils - -np.set_printoptions(precision=3, suppress=True, linewidth=10000) - - -def add_opts(parser): - parser.add_argument('--agent', type=str, default="KerasDQNAgent", help="Agent to be trained with.") - parser.add_argument('--env', type=str, default="2DDetachedCartPolev0Env", help="Environment to be trained in.") - parser.add_argument('--use-latest', action='store_true', help="Should the trainer retrain/show with the most recent save?") - parser.add_argument('--train-for', type=int, default=100, help="The number of epochs to train for.") - parser.add_argument('--test-for', type=int, default=0, help="The number of epoch to test for.") - parser.add_argument('--load-file', type=str, default=None, help="The weight file to load for training.") - parser.add_argument('--save-file', type=str, default=None, help="The weight file to save after training.") - - -class Trainer: - ''' - The trainer class helps to easily set up a gym training session using an agent(representing the learning algorithm and the gym (being the environment) - ''' - # TODO: Make training fail-safe by catching "not connected to server" and save the current state to disk (see primitive examples, they can do it) - - def __init__(self): - # initialize random seed - np.random.seed(int(time.time())) - - cid = p.connect(p.SHARED_MEMORY) # only show graphics if the browser is already running.... - self.visualize = (cid >= 0) - if cid < 0: - cid = p.connect(p.DIRECT) # If no shared memory browser is active, we switch to headless mode (DIRECT is much faster) - - def setup_exercise(self, opts): - - # setup agent - agent = agent_register.make(opts.agent, opts=opts) - - # setup environment - env = gym.make(opts.env) - if self.visualize: - mode = "human" - else: - mode = "none" - env.render(mode=mode) - - # configurations - env.seed(int(time.time())) - #env.configureActions(agent.metadata['discrete_actions']) # configure environment to accepts discrete actions - if agent.metadata['discrete_actions']: - agent.configure(env.observation_space.shape, env.action_space.n) # configure agent to use the environment properties - else: - agent.configure(env.observation_space.shape, env.action_space.shape[0]) # configure agent to use the environment properties - - if opts.use_latest: - properties = kerasrl_utils.get_latest_save("checkpoints/", opts.agent, opts.env, 0) - if properties == []: - print("No previous weight saves found for %s-%s" % (opts.agent, opts.env)) - else: - opts.load_file = "checkpoints/%s-%s-%s.h5" % (properties[0], properties[1], properties[2]) - print("Continue from [%s] " % opts.load_file) - - if opts.load_file is not None: - print("loading weights from [%s]" % opts.load_file) - agent.load_weights(opts.load_file) - - # Okay, now it's time to learn something! We visualize the training here for show, but this - # slows down training quite a lot. You can always safely abort the training prematurely using - # Ctrl + C. - agent.train(env, nb_steps=opts.train_for, visualize=self.visualize, verbosity=1) - - # After training is done, we save the final weights. - if opts.save_file is not None: - print("saving weights to [%s]" % opts.save_file) - agent.save_weights(opts.save_file, overwrite=True) - - # Finally, evaluate our algorithm. - agent.test(env, nb_episodes=opts.test_for, visualize=self.visualize) - -if __name__ == "__main__": - """ - You can also run the trainer as a main class if you want to start your own agent/environment combination. If you know your precise arguments, just run this as your main. - """ - - trainer = Trainer.Trainer() - - parser = argparse.ArgumentParser() - - # add all parsing options - Trainer.add_opts(parser) - - opts, unknown = parser.parse_known_args() # parse agent and environment to add their opts - - exec("from agents import %s" % opts.agent) # import agent type - exec("from envs import %s" % opts.env) # import env type - exec("%s.add_opts(parser)" % opts.agent) - exec("%s.add_opts(parser)" % opts.env) - - # parse arguments - opts, unknown = parser.parse_known_args() - print("OPTS", opts) - print("UNKNOWN", unknown) - - - trainer.setup_exercise(opts) - diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index bfc2c2ccc..763591dc9 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -1,6 +1,8 @@ import gym, gym.spaces, gym.utils, gym.utils.seeding import numpy as np -import pybullet as p +import pybullet +from pybullet_utils import bullet_client + from pkg_resources import parse_version class MJCFBaseBulletEnv(gym.Env): @@ -17,7 +19,7 @@ class MJCFBaseBulletEnv(gym.Env): def __init__(self, robot, render=False): self.scene = None - self.physicsClientId=-1 + self.physicsClientId = -1 self.ownsPhysicsClient = 0 self.camera = Camera() self.isRender = render @@ -40,25 +42,21 @@ class MJCFBaseBulletEnv(gym.Env): def _reset(self): if (self.physicsClientId<0): - conInfo = p.getConnectionInfo() - if (conInfo['isConnected']): - self.ownsPhysicsClient = False + self.ownsPhysicsClient = True - self.physicsClientId = 0 + + if self.isRender: + self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI) else: - self.ownsPhysicsClient = True - self.physicsClientId = p.connect(p.SHARED_MEMORY) - if (self.physicsClientId<0): - if (self.isRender): - self.physicsClientId = p.connect(p.GUI) - else: - self.physicsClientId = p.connect(p.DIRECT) - p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + self._p = bullet_client.BulletClient() + + self.physicsClientId = self._p._client + self._p.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0) if self.scene is None: - self.scene = self.create_single_player_scene() + self.scene = self.create_single_player_scene(self._p) if not self.scene.multiplayer and self.ownsPhysicsClient: - self.scene.episode_restart() + self.scene.episode_restart(self._p) self.robot.scene = self.scene @@ -66,7 +64,7 @@ class MJCFBaseBulletEnv(gym.Env): self.done = 0 self.reward = 0 dump = 0 - s = self.robot.reset() + s = self.robot.reset(self._p) self.potential = self.robot.calc_potential() return s @@ -81,20 +79,20 @@ class MJCFBaseBulletEnv(gym.Env): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz - view_matrix = p.computeViewMatrixFromYawPitchRoll( + view_matrix = self._p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, upAxisIndex=2) - proj_matrix = p.computeProjectionMatrixFOV( + proj_matrix = self._p.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width)/self._render_height, nearVal=0.1, farVal=100.0) - (_, _, px, _, _) = p.getCameraImage( + (_, _, px, _, _) = self._p.getCameraImage( width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, - renderer=p.ER_BULLET_HARDWARE_OPENGL + renderer=pybullet.ER_BULLET_HARDWARE_OPENGL ) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] @@ -104,7 +102,7 @@ class MJCFBaseBulletEnv(gym.Env): def _close(self): if (self.ownsPhysicsClient): if (self.physicsClientId>=0): - p.disconnect(self.physicsClientId) + self._p.disconnect() self.physicsClientId = -1 def HUD(self, state, a, done): @@ -130,4 +128,4 @@ class Camera: lookat = [x,y,z] distance = 10 yaw = 10 - p.resetDebugVisualizerCamera(distance, yaw, -20, lookat) + self._p.resetDebugVisualizerCamera(distance, yaw, -20, lookat) diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py index a5d704293..6b1379c16 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py @@ -4,10 +4,9 @@ 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 import gym import numpy as np -import pybullet as p import pybullet_envs import time @@ -29,17 +28,12 @@ class SmallReactivePolicy: return x def main(): + pybullet.connect(pybullet.DIRECT) env = gym.make("AntBulletEnv-v0") env.render(mode="human") pi = SmallReactivePolicy(env.observation_space, env.action_space) env.reset() - torsoId = -1 - for i in range (p.getNumBodies()): - print(p.getBodyInfo(i)) - if (p.getBodyInfo(i)[0].decode() == "torso"): - torsoId=i - print("found torso") while 1: frame = 0 @@ -57,14 +51,6 @@ def main(): frame += 1 distance=5 yaw = 0 - humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId) - 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); still_open = env.render("human") if still_open==False: diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py index 2c055f490..1b68afd6f 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time @@ -34,15 +33,6 @@ def main(): pi = SmallReactivePolicy(env.observation_space, env.action_space) #disable rendering during reset, makes loading much faster env.reset() - torsoId = -1 - for i in range (p.getNumBodies()): - print(p.getBodyInfo(i)) - if (p.getBodyInfo(i)[1].decode() == "cheetah"): - torsoId=i - print("found torso") - print(p.getNumJoints(torsoId)) - for j in range (p.getNumJoints(torsoId)): - print(p.getJointInfo(torsoId,j))#LinkState(torsoId,j)) while 1: frame = 0 @@ -55,16 +45,6 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - distance=5 - yaw = 0 - humanPos = p.getLinkState(torsoId,4)[0] - 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); still_open = env.render("human") if still_open==False: diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py index 9d33c528b..367e80299 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time @@ -35,14 +34,6 @@ def main(): pi = SmallReactivePolicy(env.observation_space, env.action_space) env.reset() - for i in range (p.getNumBodies()): - print(p.getBodyInfo(i)) - if (p.getBodyInfo(i)[1].decode() == "hopper"): - torsoId=i - print("found torso") - print(p.getNumJoints(torsoId)) - for j in range (p.getNumJoints(torsoId)): - print(p.getJointInfo(torsoId,j))#LinkState(torsoId,j)) while 1: frame = 0 score = 0 @@ -56,16 +47,6 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - distance=5 - yaw = 0 - humanPos = p.getLinkState(torsoId,4)[0] - 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); still_open = env.render("human") diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py index c85ec257b..08245626b 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time @@ -33,12 +32,6 @@ def main(): env.render(mode="human") pi = SmallReactivePolicy(env.observation_space, env.action_space) env.reset() - torsoId = -1 - for i in range (p.getNumBodies()): - print(p.getBodyInfo(i)) - if (p.getBodyInfo(i)[0].decode() == "torso"): - torsoId=i - print("found humanoid torso") while 1: frame = 0 score = 0 @@ -50,18 +43,7 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - distance=5 - yaw = 0 time.sleep(1./60.) - humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId) - 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); - still_open = env.render("human") if still_open==False: diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py index ec97383da..916094b1f 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import os.path import time @@ -41,28 +40,14 @@ def demo_run(): score = 0 restart_delay = 0 obs = env.reset() - torsoId = -1 - for i in range (p.getNumBodies()): - print(p.getBodyInfo(i)) - if (p.getBodyInfo(i)[0].decode() == "torso"): - torsoId=i - print("found humanoid torso") while 1: a = pi.act(obs) obs, r, done, _ = env.step(a) score += r frame += 1 - humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId) if (gui): time.sleep(1./60) - 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); still_open = env.render("human") diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py index 000767ea5..4bd7752ff 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py index 0369c3e19..9235d167d 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py index ec827a0c3..5ce5bae7f 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py index 950befef6..952af87ce 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py @@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir) import gym import numpy as np -import pybullet as p import pybullet_envs import time @@ -33,15 +32,6 @@ def main(): pi = SmallReactivePolicy(env.observation_space, env.action_space) env.reset() - torsoId = -1 - for i in range (p.getNumBodies()): - print(p.getBodyInfo(i)) - if (p.getBodyInfo(i)[1].decode() == "walker2d"): - torsoId=i - print("found torso") - print(p.getNumJoints(torsoId)) - for j in range (p.getNumJoints(torsoId)): - print(p.getJointInfo(torsoId,j))#LinkState(torsoId,j)) while 1: frame = 0 @@ -50,21 +40,11 @@ def main(): obs = env.reset() while 1: - time.sleep(0.01) + time.sleep(1./60.) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r frame += 1 - distance=5 - yaw = 0 - humanPos = p.getLinkState(torsoId,4)[0] - 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); still_open = env.render("human") if still_open==False: diff --git a/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py deleted file mode 100644 index abbe68339..000000000 --- a/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py +++ /dev/null @@ -1,31 +0,0 @@ -#add parent dir to find package. Only needed for source code build, pip install doesn't need it. -import os, 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_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv - -def main(): - - environment = SimpleHumanoidGymEnv(renders=True) - - environment._p.setGravity(0,0,0) - - motorsIds=[] - for motor in environment._humanoid.motor_names: - motorsIds.append(environment._p.addUserDebugParameter(motor,-1,1,0)) - - while (True): - - action=[] - for motorId in motorsIds: - action.append(environment._p.readUserDebugParameter(motorId)) - - state, reward, done, info = environment.step(action) - obs = environment.getExtendedObservation() - print("obs") - print(obs) - -if __name__=="__main__": - main() \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index 41bb8321a..6220b28cf 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -1,7 +1,7 @@ from .scene_stadium import SinglePlayerStadiumScene from .env_bases import MJCFBaseBulletEnv import numpy as np -import pybullet as p +import pybullet from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid, HumanoidFlagrun, HumanoidFlagrunHarder @@ -16,25 +16,25 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): self.stateId=-1 - def create_single_player_scene(self): - self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165/4, frame_skip=4) + def create_single_player_scene(self, bullet_client): + self.stadium_scene = SinglePlayerStadiumScene(bullet_client, gravity=9.8, timestep=0.0165/4, frame_skip=4) return self.stadium_scene def _reset(self): if (self.stateId>=0): #print("restoreState self.stateId:",self.stateId) - p.restoreState(self.stateId) + self._p.restoreState(self.stateId) r = MJCFBaseBulletEnv._reset(self) - p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) + self._p.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,0) - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene( + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(self._p, self.stadium_scene.ground_plane_mjcf) self.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in self.foot_ground_object_names]) - p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) + self._p.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,1) if (self.stateId<0): - self.stateId=p.saveState() + self.stateId=self._p.saveState() #print("saving state self.stateId:",self.stateId) @@ -155,8 +155,8 @@ class HumanoidFlagrunBulletEnv(HumanoidBulletEnv): self.robot = HumanoidFlagrun() HumanoidBulletEnv.__init__(self, self.robot) - def create_single_player_scene(self): - s = HumanoidBulletEnv.create_single_player_scene(self) + def create_single_player_scene(self, bullet_client): + s = HumanoidBulletEnv.create_single_player_scene(self, bullet_client) s.zero_at_running_strip_start_line = False return s @@ -168,8 +168,8 @@ class HumanoidFlagrunHarderBulletEnv(HumanoidBulletEnv): self.electricity_cost /= 4 # don't care that much about electricity, just stand up! HumanoidBulletEnv.__init__(self, self.robot) - def create_single_player_scene(self): - s = HumanoidBulletEnv.create_single_player_scene(self) + def create_single_player_scene(self, bullet_client): + s = HumanoidBulletEnv.create_single_player_scene(self, bullet_client) s.zero_at_running_strip_start_line = False return s diff --git a/examples/pybullet/gym/pybullet_envs/gym_manipulator_envs.py b/examples/pybullet/gym/pybullet_envs/gym_manipulator_envs.py index 2fbf1fdd7..7205a3872 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_manipulator_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_manipulator_envs.py @@ -9,8 +9,8 @@ class ReacherBulletEnv(MJCFBaseBulletEnv): self.robot = Reacher() MJCFBaseBulletEnv.__init__(self, self.robot) - def create_single_player_scene(self): - return SingleRobotEmptyScene(gravity=0.0, timestep=0.0165, frame_skip=1) + def create_single_player_scene(self, bullet_client): + return SingleRobotEmptyScene(bullet_client, gravity=0.0, timestep=0.0165, frame_skip=1) def _step(self, a): assert (not self.scene.multiplayer) @@ -43,8 +43,8 @@ class PusherBulletEnv(MJCFBaseBulletEnv): self.robot = Pusher() MJCFBaseBulletEnv.__init__(self, self.robot) - def create_single_player_scene(self): - return SingleRobotEmptyScene(gravity=9.81, timestep=0.0020, frame_skip=5) + def create_single_player_scene(self, bullet_client): + return SingleRobotEmptyScene(bullet_client, gravity=9.81, timestep=0.0020, frame_skip=5) def _step(self, a): self.robot.apply_action(a) @@ -100,8 +100,8 @@ class StrikerBulletEnv(MJCFBaseBulletEnv): self._min_strike_dist = np.inf self.strike_threshold = 0.1 - def create_single_player_scene(self): - return SingleRobotEmptyScene(gravity=9.81, timestep=0.0020, frame_skip=5) + def create_single_player_scene(self, bullet_client): + return SingleRobotEmptyScene(bullet_client, gravity=9.81, timestep=0.0020, frame_skip=5) def _step(self, a): self.robot.apply_action(a) @@ -173,8 +173,8 @@ class ThrowerBulletEnv(MJCFBaseBulletEnv): self.robot = Thrower() MJCFBaseBulletEnv.__init__(self, self.robot) - def create_single_player_scene(self): - return SingleRobotEmptyScene(gravity=0.0, timestep=0.0020, frame_skip=5) + def create_single_player_scene(self, bullet_client): + return SingleRobotEmptyScene(bullet_client, gravity=0.0, timestep=0.0020, frame_skip=5) def _step(self, a): self.robot.apply_action(a) diff --git a/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py b/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py index dc4e8a67c..f71bf9283 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py @@ -3,7 +3,7 @@ from .env_bases import MJCFBaseBulletEnv from robot_pendula import InvertedPendulum, InvertedPendulumSwingup, InvertedDoublePendulum import gym, gym.spaces, gym.utils, gym.utils.seeding import numpy as np -import pybullet as p +import pybullet import os, sys class InvertedPendulumBulletEnv(MJCFBaseBulletEnv): @@ -12,8 +12,8 @@ class InvertedPendulumBulletEnv(MJCFBaseBulletEnv): MJCFBaseBulletEnv.__init__(self, self.robot) self.stateId=-1 - def create_single_player_scene(self): - return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1) + def create_single_player_scene(self, bullet_client): + return SingleRobotEmptyScene(bullet_client, gravity=9.8, timestep=0.0165, frame_skip=1) def _reset(self): if (self.stateId>=0): @@ -21,7 +21,7 @@ class InvertedPendulumBulletEnv(MJCFBaseBulletEnv): p.restoreState(self.stateId) r = MJCFBaseBulletEnv._reset(self) if (self.stateId<0): - self.stateId = p.saveState() + self.stateId = self._p.saveState() #print("InvertedPendulumBulletEnv reset self.stateId=",self.stateId) return r @@ -54,15 +54,15 @@ class InvertedDoublePendulumBulletEnv(MJCFBaseBulletEnv): self.robot = InvertedDoublePendulum() MJCFBaseBulletEnv.__init__(self, self.robot) self.stateId = -1 - def create_single_player_scene(self): - return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1) + def create_single_player_scene(self, bullet_client): + return SingleRobotEmptyScene(bullet_client, gravity=9.8, timestep=0.0165, frame_skip=1) def _reset(self): if (self.stateId>=0): p.restoreState(self.stateId) r = MJCFBaseBulletEnv._reset(self) if (self.stateId<0): - self.stateId = p.saveState() + self.stateId = self._p.saveState() return r def _step(self, a): diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index d4059d2ac..25d079831 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -1,4 +1,4 @@ -import pybullet as p +import pybullet import gym, gym.spaces, gym.utils import numpy as np import os, inspect @@ -14,8 +14,7 @@ class XmlBasedRobot: """ self_collision = True - def __init__(self, robot_name, action_dim, obs_dim, self_collision): - #def __init__(self, model_xml, robot_name, action_dim, obs_dim): + def __init__(self, robot_name, action_dim, obs_dim, self_collision): self.parts = None self.objects = [] self.jdict = None @@ -31,7 +30,9 @@ class XmlBasedRobot: self.robot_name = robot_name self.self_collision = self_collision - def addToScene(self, bodies): + def addToScene(self, bullet_client, bodies): + self._p = bullet_client + if self.parts is not None: parts = self.parts else: @@ -52,14 +53,14 @@ class XmlBasedRobot: dump = 0 for i in range(len(bodies)): - if p.getNumJoints(bodies[i]) == 0: - part_name, robot_name = p.getBodyInfo(bodies[i], 0) + if self._p.getNumJoints(bodies[i]) == 0: + part_name, robot_name = self._p.getBodyInfo(bodies[i]) self.robot_name = robot_name.decode("utf8") part_name = part_name.decode("utf8") - parts[part_name] = BodyPart(part_name, bodies, i, -1) - for j in range(p.getNumJoints(bodies[i])): - p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0) - jointInfo = p.getJointInfo(bodies[i], j) + parts[part_name] = BodyPart(self._p, part_name, bodies, i, -1) + for j in range(self._p.getNumJoints(bodies[i])): + self._p.setJointMotorControl2(bodies[i],j,pybullet.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0) + jointInfo = self._p.getJointInfo(bodies[i], j) joint_name=jointInfo[1] part_name=jointInfo[12] @@ -69,21 +70,21 @@ class XmlBasedRobot: if dump: print("ROBOT PART '%s'" % part_name) if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) ) - parts[part_name] = BodyPart(part_name, bodies, i, j) + parts[part_name] = BodyPart(self._p, part_name, bodies, i, j) if part_name == self.robot_name: self.robot_body = parts[part_name] if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body - parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1) + parts[self.robot_name] = BodyPart(self._p, self.robot_name, bodies, 0, -1) self.robot_body = parts[self.robot_name] if joint_name[:6] == "ignore": - Joint(joint_name, bodies, i, j).disable_motor() + Joint(self._p, joint_name, bodies, i, j).disable_motor() continue if joint_name[:8] != "jointfix": - joints[joint_name] = Joint(joint_name, bodies, i, j) + joints[joint_name] = Joint(self._p, joint_name, bodies, i, j) ordered_joints.append(joints[joint_name]) joints[joint_name].power_coef = 100.0 @@ -103,23 +104,24 @@ class MJCFBasedRobot(XmlBasedRobot): Base class for mujoco .xml based agents. """ - def __init__(self, model_xml, robot_name, action_dim, obs_dim, self_collision=True): + def __init__(self, model_xml, robot_name, action_dim, obs_dim, self_collision=True): XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) self.model_xml = model_xml self.doneLoading=0 - def reset(self): - + def reset(self, bullet_client): + self._p = bullet_client + print("Created bullet_client with id=", self._p._client) if (self.doneLoading==0): self.ordered_joints = [] self.doneLoading=1 if self.self_collision: - self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects ) + self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=pybullet.URDF_USE_SELF_COLLISION|pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, self.objects ) else: - self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml)) - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects) - self.robot_specific_reset() + self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml)) + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, self.objects) + self.robot_specific_reset(self._p) s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use @@ -142,26 +144,27 @@ class URDFBasedRobot(XmlBasedRobot): self.baseOrientation = baseOrientation self.fixed_base = fixed_base - def reset(self): + def reset(self, bullet_client): + self._p = bullet_client self.ordered_joints = [] print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf)) if self.self_collision: - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( - p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, + self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base, - flags=p.URDF_USE_SELF_COLLISION)) + flags=pybullet.URDF_USE_SELF_COLLISION)) else: - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( - p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, + self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base)) - self.robot_specific_reset() + self.robot_specific_reset(self._p) s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use self.potential = self.calc_potential() @@ -177,19 +180,21 @@ class SDFBasedRobot(XmlBasedRobot): Base class for SDF robots in a Scene. """ - def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False): + def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False): XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) self.model_sdf = model_sdf self.fixed_base = fixed_base - def reset(self): + def reset(self, bullet_client): + self._p = bullet_client + self.ordered_joints = [] - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( # TODO: Not sure if this works, try it with kuka - p.loadSDF(os.path.join("models_robot", self.model_sdf))) + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, # TODO: Not sure if this works, try it with kuka + self._p.loadSDF(os.path.join("models_robot", self.model_sdf))) - self.robot_specific_reset() + self.robot_specific_reset(self._p) s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use self.potential = self.calc_potential() @@ -208,14 +213,15 @@ class Pose_Helper: # dummy class to comply to original interface return self.body_part.current_position() def rpy(self): - return p.getEulerFromQuaternion(self.body_part.current_orientation()) + return pybullet.getEulerFromQuaternion(self.body_part.current_orientation()) def orientation(self): return self.body_part.current_orientation() class BodyPart: - def __init__(self, body_name, bodies, bodyIndex, bodyPartIndex): + def __init__(self, bullet_client, body_name, bodies, bodyIndex, bodyPartIndex): self.bodies = bodies + self._p = bullet_client self.bodyIndex = bodyIndex self.bodyPartIndex = bodyPartIndex self.initialPosition = self.current_position() @@ -224,9 +230,9 @@ class BodyPart: def state_fields_of_pose_of(self, body_id, link_id=-1): # a method you will most probably need a lot to get pose and orientation if link_id == -1: - (x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id) + (x, y, z), (a, b, c, d) = self._p.getBasePositionAndOrientation(body_id) else: - (x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id) + (x, y, z), (a, b, c, d), _, _, _, _ = self._p.getLinkState(body_id, link_id) return np.array([x, y, z, a, b, c, d]) def get_pose(self): @@ -234,9 +240,9 @@ class BodyPart: def speed(self): if self.bodyPartIndex == -1: - (vx, vy, vz), _ = p.getBaseVelocity(self.bodies[self.bodyIndex]) + (vx, vy, vz), _ = self._p.getBaseVelocity(self.bodies[self.bodyIndex]) else: - (x,y,z), (a,b,c,d), _,_,_,_, (vx, vy, vz), (vr,vp,vy) = p.getLinkState(self.bodies[self.bodyIndex], self.bodyPartIndex, computeLinkVelocity=1) + (x,y,z), (a,b,c,d), _,_,_,_, (vx, vy, vz), (vr,vp,vy) = self._p.getLinkState(self.bodies[self.bodyIndex], self.bodyPartIndex, computeLinkVelocity=1) return np.array([vx, vy, vz]) def current_position(self): @@ -249,39 +255,40 @@ class BodyPart: return self.current_orientation() def reset_position(self, position): - p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation()) + self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation()) def reset_orientation(self, orientation): - p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) + self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) def reset_velocity(self, linearVelocity=[0,0,0], angularVelocity =[0,0,0]): - p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity) + self._p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity) def reset_pose(self, position, orientation): - p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation) + self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation) def pose(self): return self.bp_pose def contact_list(self): - return p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1) + return self._p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1) class Joint: - def __init__(self, joint_name, bodies, bodyIndex, jointIndex): + def __init__(self, bullet_client, joint_name, bodies, bodyIndex, jointIndex): self.bodies = bodies + self._p = bullet_client self.bodyIndex = bodyIndex self.jointIndex = jointIndex self.joint_name = joint_name - jointInfo = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex) + jointInfo = self._p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex) self.lowerLimit = jointInfo[8] self.upperLimit = jointInfo[9] self.power_coeff = 0 def set_state(self, x, vx): - p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx) + self._p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx) def current_position(self): # just some synonyme method return self.get_state() @@ -295,7 +302,7 @@ class Joint: ) def get_state(self): - x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex) + x, vx,_,_ = self._p.getJointState(self.bodies[self.bodyIndex],self.jointIndex) return x, vx def get_position(self): @@ -311,23 +318,23 @@ class Joint: return vx def set_position(self, position): - p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position) + self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,pybullet.POSITION_CONTROL, targetPosition=position) def set_velocity(self, velocity): - p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity) + self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,pybullet.VELOCITY_CONTROL, targetVelocity=velocity) def set_motor_torque(self, torque): # just some synonyme method self.set_torque(torque) def set_torque(self, torque): - p.setJointMotorControl2(bodyIndex=self.bodies[self.bodyIndex], jointIndex=self.jointIndex, controlMode=p.TORQUE_CONTROL, force=torque) #, positionGain=0.1, velocityGain=0.1) + self._p.setJointMotorControl2(bodyIndex=self.bodies[self.bodyIndex], jointIndex=self.jointIndex, controlMode=pybullet.TORQUE_CONTROL, force=torque) #, positionGain=0.1, velocityGain=0.1) def reset_current_position(self, position, velocity): # just some synonyme method self.reset_position(position, velocity) def reset_position(self, position, velocity): - p.resetJointState(self.bodies[self.bodyIndex],self.jointIndex,targetValue=position, targetVelocity=velocity) + self._p.resetJointState(self.bodies[self.bodyIndex],self.jointIndex,targetValue=position, targetVelocity=velocity) self.disable_motor() def disable_motor(self): - p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0) + self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=pybullet.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0) diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 2dbb61810..7f2d95ef1 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -1,12 +1,12 @@ from robot_bases import XmlBasedRobot, MJCFBasedRobot, URDFBasedRobot import numpy as np -import pybullet as p +import pybullet import os import pybullet_data from robot_bases import BodyPart class WalkerBase(MJCFBasedRobot): - def __init__(self, fn, robot_name, action_dim, obs_dim, power): + def __init__(self, fn, robot_name, action_dim, obs_dim, power): MJCFBasedRobot.__init__(self, fn, robot_name, action_dim, obs_dim) self.power = power self.camera_x = 0 @@ -15,7 +15,8 @@ class WalkerBase(MJCFBasedRobot): self.walk_target_y = 0 self.body_xyz=[0,0,0] - def robot_specific_reset(self): + def robot_specific_reset(self, bullet_client): + self._p = bullet_client for j in self.ordered_joints: j.reset_current_position(self.np_random.uniform(low=-0.1, high=0.1), 0) @@ -94,13 +95,13 @@ class Walker2D(WalkerBase): foot_list = ["foot", "foot_left"] def __init__(self): - WalkerBase.__init__(self, "walker2d.xml", "torso", action_dim=6, obs_dim=22, power=0.40) + WalkerBase.__init__(self, "walker2d.xml", "torso", action_dim=6, obs_dim=22, power=0.40) def alive_bonus(self, z, pitch): return +1 if z > 0.8 and abs(pitch) < 1.0 else -1 - def robot_specific_reset(self): - WalkerBase.robot_specific_reset(self) + def robot_specific_reset(self, bullet_client): + WalkerBase.robot_specific_reset(self, bullet_client) for n in ["foot_joint", "foot_left_joint"]: self.jdict[n].power_coef = 30.0 @@ -140,11 +141,11 @@ class Humanoid(WalkerBase): foot_list = ["right_foot", "left_foot"] # "left_hand", "right_hand" def __init__(self): - WalkerBase.__init__(self, 'humanoid_symmetric.xml', 'torso', action_dim=17, obs_dim=44, power=0.41) + WalkerBase.__init__(self, 'humanoid_symmetric.xml', 'torso', action_dim=17, obs_dim=44, power=0.41) # 17 joints, 4 of them important for walking (hip, knee), others may as well be turned off, 17/4 = 4.25 - def robot_specific_reset(self): - WalkerBase.robot_specific_reset(self) + def robot_specific_reset(self, bullet_client): + WalkerBase.robot_specific_reset(self, bullet_client) self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"] self.motor_power = [100, 100, 100] self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"] @@ -192,29 +193,29 @@ class Humanoid(WalkerBase): -def get_cube(x, y, z): - body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z) - p.changeDynamics(body,-1, mass=1.2)#match Roboschool - part_name, _ = p.getBodyInfo(body, 0) +def get_cube(_p, x, y, z): + body = _p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), [x, y, z]) + _p.changeDynamics(body,-1, mass=1.2)#match Roboschool + part_name, _ = _p.getBodyInfo(body) part_name = part_name.decode("utf8") bodies = [body] - return BodyPart(part_name, bodies, 0, -1) + return BodyPart(_p, part_name, bodies, 0, -1) -def get_sphere(x, y, z): - body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"sphere2red_nocol.urdf"), x, y, z) - part_name, _ = p.getBodyInfo(body, 0) +def get_sphere(_p, x, y, z): + body = _p.loadURDF(os.path.join(pybullet_data.getDataPath(),"sphere2red_nocol.urdf"), [x, y, z]) + part_name, _ = _p.getBodyInfo(body) part_name = part_name.decode("utf8") bodies = [body] - return BodyPart(part_name, bodies, 0, -1) + return BodyPart(_p, part_name, bodies, 0, -1) class HumanoidFlagrun(Humanoid): def __init__(self): Humanoid.__init__(self) self.flag = None - def robot_specific_reset(self): - Humanoid.robot_specific_reset(self) + def robot_specific_reset(self, bullet_client): + Humanoid.robot_specific_reset(self, bullet_client) self.flag_reposition() def flag_reposition(self): @@ -228,9 +229,9 @@ class HumanoidFlagrun(Humanoid): #for b in self.flag.bodies: # print("remove body uid",b) # p.removeBody(b) - p.resetBasePositionAndOrientation(self.flag.bodies[0],[self.walk_target_x, self.walk_target_y, 0.7],[0,0,0,1]) + self._p.resetBasePositionAndOrientation(self.flag.bodies[0],[self.walk_target_x, self.walk_target_y, 0.7],[0,0,0,1]) else: - self.flag = get_sphere(self.walk_target_x, self.walk_target_y, 0.7) + self.flag = get_sphere(self._p, self.walk_target_x, self.walk_target_y, 0.7) self.flag_timeout = 600/self.scene.frame_skip #match Roboschool def calc_state(self): @@ -250,14 +251,15 @@ class HumanoidFlagrunHarder(HumanoidFlagrun): self.aggressive_cube = None self.frame = 0 - def robot_specific_reset(self): - HumanoidFlagrun.robot_specific_reset(self) + def robot_specific_reset(self, bullet_client): + + HumanoidFlagrun.robot_specific_reset(self, bullet_client) self.frame = 0 if (self.aggressive_cube): - p.resetBasePositionAndOrientation(self.aggressive_cube.bodies[0],[-1.5,0,0.05],[0,0,0,1]) + self._p.resetBasePositionAndOrientation(self.aggressive_cube.bodies[0],[-1.5,0,0.05],[0,0,0,1]) else: - self.aggressive_cube = get_cube(-1.5,0,0.05) + self.aggressive_cube = get_cube(self._p, -1.5,0,0.05) self.on_ground_frame_counter = 0 self.crawl_start_potential = None self.crawl_ignored_potential = 0.0 diff --git a/examples/pybullet/gym/pybullet_envs/robot_pendula.py b/examples/pybullet/gym/pybullet_envs/robot_pendula.py index 39c2c3878..a90728128 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_pendula.py +++ b/examples/pybullet/gym/pybullet_envs/robot_pendula.py @@ -6,7 +6,8 @@ class InvertedPendulum(MJCFBasedRobot): def __init__(self): MJCFBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5) - def robot_specific_reset(self): + def robot_specific_reset(self, bullet_client): + self._p = bullet_client self.pole = self.parts["pole"] self.slider = self.jdict["slider"] self.j1 = self.jdict["hinge"] @@ -55,7 +56,8 @@ class InvertedDoublePendulum(MJCFBasedRobot): def __init__(self): MJCFBasedRobot.__init__(self, 'inverted_double_pendulum.xml', 'cart', action_dim=1, obs_dim=9) - def robot_specific_reset(self): + def robot_specific_reset(self, bullet_client): + self._p = bullet_client self.pole2 = self.parts["pole2"] self.slider = self.jdict["slider"] self.j1 = self.jdict["hinge"] diff --git a/examples/pybullet/gym/pybullet_envs/scene_abstract.py b/examples/pybullet/gym/pybullet_envs/scene_abstract.py index e550abdd8..cc74839c8 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_abstract.py +++ b/examples/pybullet/gym/pybullet_envs/scene_abstract.py @@ -1,6 +1,6 @@ import sys, os sys.path.append(os.path.dirname(__file__)) -import pybullet as p +import pybullet import gym @@ -8,13 +8,14 @@ import gym class Scene: "A base class for single- and multiplayer scenes" - def __init__(self, gravity, timestep, frame_skip): + def __init__(self, bullet_client, gravity, timestep, frame_skip): + self._p = bullet_client self.np_random, seed = gym.utils.seeding.np_random(None) self.timestep = timestep self.frame_skip = frame_skip self.dt = self.timestep * self.frame_skip - self.cpp_world = World(gravity, timestep, frame_skip) + self.cpp_world = World(self._p, gravity, timestep, frame_skip) self.test_window_still_open = True # or never opened self.human_render_detected = False # if user wants render("human"), we open test window @@ -55,7 +56,8 @@ class SingleRobotEmptyScene(Scene): class World: - def __init__(self, gravity, timestep, frame_skip): + def __init__(self, bullet_client, gravity, timestep, frame_skip): + self._p = bullet_client self.gravity = gravity self.timestep = timestep self.frame_skip = frame_skip @@ -65,12 +67,12 @@ class World: def clean_everything(self): #p.resetSimulation() - p.setGravity(0, 0, -self.gravity) - p.setDefaultContactERP(0.9) + self._p.setGravity(0, 0, -self.gravity) + self._p.setDefaultContactERP(0.9) #print("self.numSolverIterations=",self.numSolverIterations) - p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=self.numSolverIterations, numSubSteps=self.frame_skip) + self._p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=self.numSolverIterations, numSubSteps=self.frame_skip) def step(self, frame_skip): - p.stepSimulation() + self._p.stepSimulation() diff --git a/examples/pybullet/gym/pybullet_envs/scene_stadium.py b/examples/pybullet/gym/pybullet_envs/scene_stadium.py index 6e64ff623..8aef2552c 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_stadium.py +++ b/examples/pybullet/gym/pybullet_envs/scene_stadium.py @@ -6,7 +6,7 @@ import pybullet_data from pybullet_envs.scene_abstract import Scene -import pybullet as p +import pybullet class StadiumScene(Scene): @@ -15,8 +15,8 @@ class StadiumScene(Scene): stadium_halfwidth = 50*0.25 # FOOBALL_FIELD_HALFWID stadiumLoaded=0 - def episode_restart(self): - + def episode_restart(self, bullet_client): + self._p = bullet_client Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 @@ -26,17 +26,17 @@ class StadiumScene(Scene): # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"plane_stadium.sdf") - self.ground_plane_mjcf=p.loadSDF(filename) + self.ground_plane_mjcf=self._p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") - #self.ground_plane_mjcf = p.loadSDF(filename) + #self.ground_plane_mjcf = self._p.loadSDF(filename) # for i in self.ground_plane_mjcf: - p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) - p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) - p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) + self._p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) + self._p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) + self._p.configureDebugVisualizer(pybullet.COV_ENABLE_PLANAR_REFLECTION,1) # for j in range(p.getNumJoints(i)): - # p.changeDynamics(i,j,lateralFriction=0) + # self._p.changeDynamics(i,j,lateralFriction=0) #despite the name (stadium_no_collision), it DID have collision, so don't add duplicate ground class SinglePlayerStadiumScene(StadiumScene):