Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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.

Browse Source
This commit is contained in:
Erwin Coumans
2018-05-18 16:23:54 -07:00
parent d17d496f97
commit 0abe4151e5
24 changed files with 163 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
examples/pybullet/gym/pybullet_data/policies/__init__.py Normal file
View File

0
examples/pybullet/gym/pybullet_data/policies/ppo/__init__.py Normal file
View File

0
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/__init__.py Normal file
View File

0
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/__init__.py Normal file
View File

111
examples/pybullet/gym/pybullet_envs/Trainer.py
View File

@@ -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)

42
examples/pybullet/gym/pybullet_envs/env_bases.py
View File

@@ -1,6 +1,8 @@
import gym, gym.spaces, gym.utils, gym.utils.seeding import gym, gym.spaces, gym.utils, gym.utils.seeding
import numpy as np import numpy as np
import pybullet as p import pybullet
from pybullet_utils import bullet_client
from pkg_resources import parse_version from pkg_resources import parse_version
class MJCFBaseBulletEnv(gym.Env): class MJCFBaseBulletEnv(gym.Env):
@@ -40,25 +42,21 @@ class MJCFBaseBulletEnv(gym.Env):
def _reset(self): def _reset(self):
if (self.physicsClientId<0): if (self.physicsClientId<0):
conInfo = p.getConnectionInfo()
if (conInfo['isConnected']):
self.ownsPhysicsClient = False
self.physicsClientId = 0
else:
self.ownsPhysicsClient = True self.ownsPhysicsClient = True
self.physicsClientId = p.connect(p.SHARED_MEMORY)
if (self.physicsClientId<0):
if (self.isRender): if self.isRender:
self.physicsClientId = p.connect(p.GUI) self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else: else:
self.physicsClientId = p.connect(p.DIRECT) self._p = bullet_client.BulletClient()
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
self.physicsClientId = self._p._client
self._p.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
if self.scene is None: 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: if not self.scene.multiplayer and self.ownsPhysicsClient:
self.scene.episode_restart() self.scene.episode_restart(self._p)
self.robot.scene = self.scene self.robot.scene = self.scene
@@ -66,7 +64,7 @@ class MJCFBaseBulletEnv(gym.Env):
self.done = 0 self.done = 0
self.reward = 0 self.reward = 0
dump = 0 dump = 0
s = self.robot.reset() s = self.robot.reset(self._p)
self.potential = self.robot.calc_potential() self.potential = self.robot.calc_potential()
return s return s
@@ -81,20 +79,20 @@ class MJCFBaseBulletEnv(gym.Env):
if (hasattr(self.robot,'body_xyz')): if (hasattr(self.robot,'body_xyz')):
base_pos = self.robot.body_xyz base_pos = self.robot.body_xyz
view_matrix = p.computeViewMatrixFromYawPitchRoll( view_matrix = self._p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos, cameraTargetPosition=base_pos,
distance=self._cam_dist, distance=self._cam_dist,
yaw=self._cam_yaw, yaw=self._cam_yaw,
pitch=self._cam_pitch, pitch=self._cam_pitch,
roll=0, roll=0,
upAxisIndex=2) upAxisIndex=2)
proj_matrix = p.computeProjectionMatrixFOV( proj_matrix = self._p.computeProjectionMatrixFOV(
fov=60, aspect=float(self._render_width)/self._render_height, fov=60, aspect=float(self._render_width)/self._render_height,
nearVal=0.1, farVal=100.0) nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = p.getCameraImage( (_, _, px, _, _) = self._p.getCameraImage(
width=self._render_width, height=self._render_height, viewMatrix=view_matrix, width=self._render_width, height=self._render_height, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, projectionMatrix=proj_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL renderer=pybullet.ER_BULLET_HARDWARE_OPENGL
) )
rgb_array = np.array(px) rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3] rgb_array = rgb_array[:, :, :3]
@@ -104,7 +102,7 @@ class MJCFBaseBulletEnv(gym.Env):
def _close(self): def _close(self):
if (self.ownsPhysicsClient): if (self.ownsPhysicsClient):
if (self.physicsClientId>=0): if (self.physicsClientId>=0):
p.disconnect(self.physicsClientId) self._p.disconnect()
self.physicsClientId = -1 self.physicsClientId = -1
def HUD(self, state, a, done): def HUD(self, state, a, done):
@@ -130,4 +128,4 @@ class Camera:
lookat = [x,y,z] lookat = [x,y,z]
distance = 10 distance = 10
yaw = 10 yaw = 10
p.resetDebugVisualizerCamera(distance, yaw, -20, lookat) self._p.resetDebugVisualizerCamera(distance, yaw, -20, lookat)

18
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py
View File

@@ -4,10 +4,9 @@ import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir)) parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir) os.sys.path.insert(0,parentdir)
import pybullet
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time
@@ -29,17 +28,12 @@ class SmallReactivePolicy:
return x return x
def main(): def main():
pybullet.connect(pybullet.DIRECT)
env = gym.make("AntBulletEnv-v0") env = gym.make("AntBulletEnv-v0")
env.render(mode="human") env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space) pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset() 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: while 1:
frame = 0 frame = 0
@@ -57,14 +51,6 @@ def main():
frame += 1 frame += 1
distance=5 distance=5
yaw = 0 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") still_open = env.render("human")
if still_open==False: if still_open==False:

20
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time
@@ -34,15 +33,6 @@ def main():
pi = SmallReactivePolicy(env.observation_space, env.action_space) pi = SmallReactivePolicy(env.observation_space, env.action_space)
#disable rendering during reset, makes loading much faster #disable rendering during reset, makes loading much faster
env.reset() 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: while 1:
frame = 0 frame = 0
@@ -55,16 +45,6 @@ def main():
obs, r, done, _ = env.step(a) obs, r, done, _ = env.step(a)
score += r score += r
frame += 1 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") still_open = env.render("human")
if still_open==False: if still_open==False:

19
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time
@@ -35,14 +34,6 @@ def main():
pi = SmallReactivePolicy(env.observation_space, env.action_space) pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset() 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: while 1:
frame = 0 frame = 0
score = 0 score = 0
@@ -56,16 +47,6 @@ def main():
obs, r, done, _ = env.step(a) obs, r, done, _ = env.step(a)
score += r score += r
frame += 1 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") still_open = env.render("human")

18
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time
@@ -33,12 +32,6 @@ def main():
env.render(mode="human") env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space) pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset() 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: while 1:
frame = 0 frame = 0
score = 0 score = 0
@@ -50,18 +43,7 @@ def main():
obs, r, done, _ = env.step(a) obs, r, done, _ = env.step(a)
score += r score += r
frame += 1 frame += 1
distance=5
yaw = 0
time.sleep(1./60.) 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") still_open = env.render("human")
if still_open==False: if still_open==False:

15
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import os.path import os.path
import time import time
@@ -41,28 +40,14 @@ def demo_run():
score = 0 score = 0
restart_delay = 0 restart_delay = 0
obs = env.reset() 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: while 1:
a = pi.act(obs) a = pi.act(obs)
obs, r, done, _ = env.step(a) obs, r, done, _ = env.step(a)
score += r score += r
frame += 1 frame += 1
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
if (gui): if (gui):
time.sleep(1./60) 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") still_open = env.render("human")

1
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time

1
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time

1
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time

22
examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py
View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym import gym
import numpy as np import numpy as np
import pybullet as p
import pybullet_envs import pybullet_envs
import time import time
@@ -33,15 +32,6 @@ def main():
pi = SmallReactivePolicy(env.observation_space, env.action_space) pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset() 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: while 1:
frame = 0 frame = 0
@@ -50,21 +40,11 @@ def main():
obs = env.reset() obs = env.reset()
while 1: while 1:
time.sleep(0.01) time.sleep(1./60.)
a = pi.act(obs) a = pi.act(obs)
obs, r, done, _ = env.step(a) obs, r, done, _ = env.step(a)
score += r score += r
frame += 1 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") still_open = env.render("human")
if still_open==False: if still_open==False:

31
examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py
View File

@@ -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()

24
examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py
View File

@@ -1,7 +1,7 @@
from .scene_stadium import SinglePlayerStadiumScene from .scene_stadium import SinglePlayerStadiumScene
from .env_bases import MJCFBaseBulletEnv from .env_bases import MJCFBaseBulletEnv
import numpy as np import numpy as np
import pybullet as p import pybullet
from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid, HumanoidFlagrun, HumanoidFlagrunHarder from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid, HumanoidFlagrun, HumanoidFlagrunHarder
@@ -16,25 +16,25 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
self.stateId=-1 self.stateId=-1
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165/4, frame_skip=4) self.stadium_scene = SinglePlayerStadiumScene(bullet_client, gravity=9.8, timestep=0.0165/4, frame_skip=4)
return self.stadium_scene return self.stadium_scene
def _reset(self): def _reset(self):
if (self.stateId>=0): if (self.stateId>=0):
#print("restoreState self.stateId:",self.stateId) #print("restoreState self.stateId:",self.stateId)
p.restoreState(self.stateId) self._p.restoreState(self.stateId)
r = MJCFBaseBulletEnv._reset(self) 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.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.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in
self.foot_ground_object_names]) self.foot_ground_object_names])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) self._p.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING,1)
if (self.stateId<0): if (self.stateId<0):
self.stateId=p.saveState() self.stateId=self._p.saveState()
#print("saving state self.stateId:",self.stateId) #print("saving state self.stateId:",self.stateId)
@@ -155,8 +155,8 @@ class HumanoidFlagrunBulletEnv(HumanoidBulletEnv):
self.robot = HumanoidFlagrun() self.robot = HumanoidFlagrun()
HumanoidBulletEnv.__init__(self, self.robot) HumanoidBulletEnv.__init__(self, self.robot)
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
s = HumanoidBulletEnv.create_single_player_scene(self) s = HumanoidBulletEnv.create_single_player_scene(self, bullet_client)
s.zero_at_running_strip_start_line = False s.zero_at_running_strip_start_line = False
return s return s
@@ -168,8 +168,8 @@ class HumanoidFlagrunHarderBulletEnv(HumanoidBulletEnv):
self.electricity_cost /= 4 # don't care that much about electricity, just stand up! self.electricity_cost /= 4 # don't care that much about electricity, just stand up!
HumanoidBulletEnv.__init__(self, self.robot) HumanoidBulletEnv.__init__(self, self.robot)
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
s = HumanoidBulletEnv.create_single_player_scene(self) s = HumanoidBulletEnv.create_single_player_scene(self, bullet_client)
s.zero_at_running_strip_start_line = False s.zero_at_running_strip_start_line = False
return s return s

16
examples/pybullet/gym/pybullet_envs/gym_manipulator_envs.py
View File

@@ -9,8 +9,8 @@ class ReacherBulletEnv(MJCFBaseBulletEnv):
self.robot = Reacher() self.robot = Reacher()
MJCFBaseBulletEnv.__init__(self, self.robot) MJCFBaseBulletEnv.__init__(self, self.robot)
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
return SingleRobotEmptyScene(gravity=0.0, timestep=0.0165, frame_skip=1) return SingleRobotEmptyScene(bullet_client, gravity=0.0, timestep=0.0165, frame_skip=1)
def _step(self, a): def _step(self, a):
assert (not self.scene.multiplayer) assert (not self.scene.multiplayer)
@@ -43,8 +43,8 @@ class PusherBulletEnv(MJCFBaseBulletEnv):
self.robot = Pusher() self.robot = Pusher()
MJCFBaseBulletEnv.__init__(self, self.robot) MJCFBaseBulletEnv.__init__(self, self.robot)
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
return SingleRobotEmptyScene(gravity=9.81, timestep=0.0020, frame_skip=5) return SingleRobotEmptyScene(bullet_client, gravity=9.81, timestep=0.0020, frame_skip=5)
def _step(self, a): def _step(self, a):
self.robot.apply_action(a) self.robot.apply_action(a)
@@ -100,8 +100,8 @@ class StrikerBulletEnv(MJCFBaseBulletEnv):
self._min_strike_dist = np.inf self._min_strike_dist = np.inf
self.strike_threshold = 0.1 self.strike_threshold = 0.1
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
return SingleRobotEmptyScene(gravity=9.81, timestep=0.0020, frame_skip=5) return SingleRobotEmptyScene(bullet_client, gravity=9.81, timestep=0.0020, frame_skip=5)
def _step(self, a): def _step(self, a):
self.robot.apply_action(a) self.robot.apply_action(a)
@@ -173,8 +173,8 @@ class ThrowerBulletEnv(MJCFBaseBulletEnv):
self.robot = Thrower() self.robot = Thrower()
MJCFBaseBulletEnv.__init__(self, self.robot) MJCFBaseBulletEnv.__init__(self, self.robot)
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
return SingleRobotEmptyScene(gravity=0.0, timestep=0.0020, frame_skip=5) return SingleRobotEmptyScene(bullet_client, gravity=0.0, timestep=0.0020, frame_skip=5)
def _step(self, a): def _step(self, a):
self.robot.apply_action(a) self.robot.apply_action(a)

14
examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py
View File

@@ -3,7 +3,7 @@ from .env_bases import MJCFBaseBulletEnv
from robot_pendula import InvertedPendulum, InvertedPendulumSwingup, InvertedDoublePendulum from robot_pendula import InvertedPendulum, InvertedPendulumSwingup, InvertedDoublePendulum
import gym, gym.spaces, gym.utils, gym.utils.seeding import gym, gym.spaces, gym.utils, gym.utils.seeding
import numpy as np import numpy as np
import pybullet as p import pybullet
import os, sys import os, sys
class InvertedPendulumBulletEnv(MJCFBaseBulletEnv): class InvertedPendulumBulletEnv(MJCFBaseBulletEnv):
@@ -12,8 +12,8 @@ class InvertedPendulumBulletEnv(MJCFBaseBulletEnv):
MJCFBaseBulletEnv.__init__(self, self.robot) MJCFBaseBulletEnv.__init__(self, self.robot)
self.stateId=-1 self.stateId=-1
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1) return SingleRobotEmptyScene(bullet_client, gravity=9.8, timestep=0.0165, frame_skip=1)
def _reset(self): def _reset(self):
if (self.stateId>=0): if (self.stateId>=0):
@@ -21,7 +21,7 @@ class InvertedPendulumBulletEnv(MJCFBaseBulletEnv):
p.restoreState(self.stateId) p.restoreState(self.stateId)
r = MJCFBaseBulletEnv._reset(self) r = MJCFBaseBulletEnv._reset(self)
if (self.stateId<0): if (self.stateId<0):
self.stateId = p.saveState() self.stateId = self._p.saveState()
#print("InvertedPendulumBulletEnv reset self.stateId=",self.stateId) #print("InvertedPendulumBulletEnv reset self.stateId=",self.stateId)
return r return r
@@ -54,15 +54,15 @@ class InvertedDoublePendulumBulletEnv(MJCFBaseBulletEnv):
self.robot = InvertedDoublePendulum() self.robot = InvertedDoublePendulum()
MJCFBaseBulletEnv.__init__(self, self.robot) MJCFBaseBulletEnv.__init__(self, self.robot)
self.stateId = -1 self.stateId = -1
def create_single_player_scene(self): def create_single_player_scene(self, bullet_client):
return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1) return SingleRobotEmptyScene(bullet_client, gravity=9.8, timestep=0.0165, frame_skip=1)
def _reset(self): def _reset(self):
if (self.stateId>=0): if (self.stateId>=0):
p.restoreState(self.stateId) p.restoreState(self.stateId)
r = MJCFBaseBulletEnv._reset(self) r = MJCFBaseBulletEnv._reset(self)
if (self.stateId<0): if (self.stateId<0):
self.stateId = p.saveState() self.stateId = self._p.saveState()
return r return r
def _step(self, a): def _step(self, a):

109
examples/pybullet/gym/pybullet_envs/robot_bases.py
View File

@@ -1,4 +1,4 @@
import pybullet as p import pybullet
import gym, gym.spaces, gym.utils import gym, gym.spaces, gym.utils
import numpy as np import numpy as np
import os, inspect import os, inspect
@@ -15,7 +15,6 @@ class XmlBasedRobot:
self_collision = True self_collision = True
def __init__(self, robot_name, action_dim, obs_dim, self_collision): def __init__(self, robot_name, action_dim, obs_dim, self_collision):
#def __init__(self, model_xml, robot_name, action_dim, obs_dim):
self.parts = None self.parts = None
self.objects = [] self.objects = []
self.jdict = None self.jdict = None
@@ -31,7 +30,9 @@ class XmlBasedRobot:
self.robot_name = robot_name self.robot_name = robot_name
self.self_collision = self_collision 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: if self.parts is not None:
parts = self.parts parts = self.parts
else: else:
@@ -52,14 +53,14 @@ class XmlBasedRobot:
dump = 0 dump = 0
for i in range(len(bodies)): for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0: if self._p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0) part_name, robot_name = self._p.getBodyInfo(bodies[i])
self.robot_name = robot_name.decode("utf8") self.robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8") part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1) parts[part_name] = BodyPart(self._p, part_name, bodies, i, -1)
for j in range(p.getNumJoints(bodies[i])): for j in range(self._p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0) self._p.setJointMotorControl2(bodies[i],j,pybullet.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
jointInfo = p.getJointInfo(bodies[i], j) jointInfo = self._p.getJointInfo(bodies[i], j)
joint_name=jointInfo[1] joint_name=jointInfo[1]
part_name=jointInfo[12] part_name=jointInfo[12]
@@ -69,21 +70,21 @@ class XmlBasedRobot:
if dump: print("ROBOT PART '%s'" % part_name) 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()) ) 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: if part_name == self.robot_name:
self.robot_body = parts[part_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 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] self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore": if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j).disable_motor() Joint(self._p, joint_name, bodies, i, j).disable_motor()
continue continue
if joint_name[:8] != "jointfix": 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]) ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0 joints[joint_name].power_coef = 100.0
@@ -107,19 +108,20 @@ class MJCFBasedRobot(XmlBasedRobot):
XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision)
self.model_xml = model_xml self.model_xml = model_xml
self.doneLoading=0 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): if (self.doneLoading==0):
self.ordered_joints = [] self.ordered_joints = []
self.doneLoading=1 self.doneLoading=1
if self.self_collision: 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.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.objects ) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, self.objects )
else: else:
self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml)) 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.objects) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, self.objects)
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 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.baseOrientation = baseOrientation
self.fixed_base = fixed_base self.fixed_base = fixed_base
def reset(self): def reset(self, bullet_client):
self._p = bullet_client
self.ordered_joints = [] self.ordered_joints = []
print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf)) print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf))
if self.self_collision: if self.self_collision:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p,
p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
basePosition=self.basePosition, basePosition=self.basePosition,
baseOrientation=self.baseOrientation, baseOrientation=self.baseOrientation,
useFixedBase=self.fixed_base, useFixedBase=self.fixed_base,
flags=p.URDF_USE_SELF_COLLISION)) flags=pybullet.URDF_USE_SELF_COLLISION))
else: else:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p,
p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
basePosition=self.basePosition, basePosition=self.basePosition,
baseOrientation=self.baseOrientation, baseOrientation=self.baseOrientation,
useFixedBase=self.fixed_base)) 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 s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential() self.potential = self.calc_potential()
@@ -183,13 +186,15 @@ class SDFBasedRobot(XmlBasedRobot):
self.model_sdf = model_sdf self.model_sdf = model_sdf
self.fixed_base = fixed_base self.fixed_base = fixed_base
def reset(self): def reset(self, bullet_client):
self._p = bullet_client
self.ordered_joints = [] 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 self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, # TODO: Not sure if this works, try it with kuka
p.loadSDF(os.path.join("models_robot", self.model_sdf))) 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 s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential() 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() return self.body_part.current_position()
def rpy(self): def rpy(self):
return p.getEulerFromQuaternion(self.body_part.current_orientation()) return pybullet.getEulerFromQuaternion(self.body_part.current_orientation())
def orientation(self): def orientation(self):
return self.body_part.current_orientation() return self.body_part.current_orientation()
class BodyPart: class BodyPart:
def __init__(self, body_name, bodies, bodyIndex, bodyPartIndex): def __init__(self, bullet_client, body_name, bodies, bodyIndex, bodyPartIndex):
self.bodies = bodies self.bodies = bodies
self._p = bullet_client
self.bodyIndex = bodyIndex self.bodyIndex = bodyIndex
self.bodyPartIndex = bodyPartIndex self.bodyPartIndex = bodyPartIndex
self.initialPosition = self.current_position() 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 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: 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: 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]) return np.array([x, y, z, a, b, c, d])
def get_pose(self): def get_pose(self):
@@ -234,9 +240,9 @@ class BodyPart:
def speed(self): def speed(self):
if self.bodyPartIndex == -1: if self.bodyPartIndex == -1:
(vx, vy, vz), _ = p.getBaseVelocity(self.bodies[self.bodyIndex]) (vx, vy, vz), _ = self._p.getBaseVelocity(self.bodies[self.bodyIndex])
else: 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]) return np.array([vx, vy, vz])
def current_position(self): def current_position(self):
@@ -249,39 +255,40 @@ class BodyPart:
return self.current_orientation() return self.current_orientation()
def reset_position(self, position): 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): 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]): 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): 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): def pose(self):
return self.bp_pose return self.bp_pose
def contact_list(self): 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: class Joint:
def __init__(self, joint_name, bodies, bodyIndex, jointIndex): def __init__(self, bullet_client, joint_name, bodies, bodyIndex, jointIndex):
self.bodies = bodies self.bodies = bodies
self._p = bullet_client
self.bodyIndex = bodyIndex self.bodyIndex = bodyIndex
self.jointIndex = jointIndex self.jointIndex = jointIndex
self.joint_name = joint_name 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.lowerLimit = jointInfo[8]
self.upperLimit = jointInfo[9] self.upperLimit = jointInfo[9]
self.power_coeff = 0 self.power_coeff = 0
def set_state(self, x, vx): 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 def current_position(self): # just some synonyme method
return self.get_state() return self.get_state()
@@ -295,7 +302,7 @@ class Joint:
) )
def get_state(self): 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 return x, vx
def get_position(self): def get_position(self):
@@ -311,23 +318,23 @@ class Joint:
return vx return vx
def set_position(self, position): 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): 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 def set_motor_torque(self, torque): # just some synonyme method
self.set_torque(torque) self.set_torque(torque)
def set_torque(self, 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 def reset_current_position(self, position, velocity): # just some synonyme method
self.reset_position(position, velocity) self.reset_position(position, velocity)
def reset_position(self, 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() self.disable_motor()
def disable_motor(self): 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)

48
examples/pybullet/gym/pybullet_envs/robot_locomotors.py
View File

@@ -1,6 +1,6 @@
from robot_bases import XmlBasedRobot, MJCFBasedRobot, URDFBasedRobot from robot_bases import XmlBasedRobot, MJCFBasedRobot, URDFBasedRobot
import numpy as np import numpy as np
import pybullet as p import pybullet
import os import os
import pybullet_data import pybullet_data
from robot_bases import BodyPart from robot_bases import BodyPart
@@ -15,7 +15,8 @@ class WalkerBase(MJCFBasedRobot):
self.walk_target_y = 0 self.walk_target_y = 0
self.body_xyz=[0,0,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: for j in self.ordered_joints:
j.reset_current_position(self.np_random.uniform(low=-0.1, high=0.1), 0) j.reset_current_position(self.np_random.uniform(low=-0.1, high=0.1), 0)
@@ -99,8 +100,8 @@ class Walker2D(WalkerBase):
def alive_bonus(self, z, pitch): def alive_bonus(self, z, pitch):
return +1 if z > 0.8 and abs(pitch) < 1.0 else -1 return +1 if z > 0.8 and abs(pitch) < 1.0 else -1
def robot_specific_reset(self): def robot_specific_reset(self, bullet_client):
WalkerBase.robot_specific_reset(self) WalkerBase.robot_specific_reset(self, bullet_client)
for n in ["foot_joint", "foot_left_joint"]: for n in ["foot_joint", "foot_left_joint"]:
self.jdict[n].power_coef = 30.0 self.jdict[n].power_coef = 30.0
@@ -143,8 +144,8 @@ class Humanoid(WalkerBase):
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 # 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): def robot_specific_reset(self, bullet_client):
WalkerBase.robot_specific_reset(self) WalkerBase.robot_specific_reset(self, bullet_client)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"] self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100] self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"] 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): def get_cube(_p, x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), 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 _p.changeDynamics(body,-1, mass=1.2)#match Roboschool
part_name, _ = p.getBodyInfo(body, 0) part_name, _ = _p.getBodyInfo(body)
part_name = part_name.decode("utf8") part_name = part_name.decode("utf8")
bodies = [body] bodies = [body]
return BodyPart(part_name, bodies, 0, -1) return BodyPart(_p, part_name, bodies, 0, -1)
def get_sphere(x, y, z): def get_sphere(_p, x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"sphere2red_nocol.urdf"), x, y, z) body = _p.loadURDF(os.path.join(pybullet_data.getDataPath(),"sphere2red_nocol.urdf"), [x, y, z])
part_name, _ = p.getBodyInfo(body, 0) part_name, _ = _p.getBodyInfo(body)
part_name = part_name.decode("utf8") part_name = part_name.decode("utf8")
bodies = [body] bodies = [body]
return BodyPart(part_name, bodies, 0, -1) return BodyPart(_p, part_name, bodies, 0, -1)
class HumanoidFlagrun(Humanoid): class HumanoidFlagrun(Humanoid):
def __init__(self): def __init__(self):
Humanoid.__init__(self) Humanoid.__init__(self)
self.flag = None self.flag = None
def robot_specific_reset(self): def robot_specific_reset(self, bullet_client):
Humanoid.robot_specific_reset(self) Humanoid.robot_specific_reset(self, bullet_client)
self.flag_reposition() self.flag_reposition()
def flag_reposition(self): def flag_reposition(self):
@@ -228,9 +229,9 @@ class HumanoidFlagrun(Humanoid):
#for b in self.flag.bodies: #for b in self.flag.bodies:
# print("remove body uid",b) # print("remove body uid",b)
# p.removeBody(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: 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 self.flag_timeout = 600/self.scene.frame_skip #match Roboschool
def calc_state(self): def calc_state(self):
@@ -250,14 +251,15 @@ class HumanoidFlagrunHarder(HumanoidFlagrun):
self.aggressive_cube = None self.aggressive_cube = None
self.frame = 0 self.frame = 0
def robot_specific_reset(self): def robot_specific_reset(self, bullet_client):
HumanoidFlagrun.robot_specific_reset(self)
HumanoidFlagrun.robot_specific_reset(self, bullet_client)
self.frame = 0 self.frame = 0
if (self.aggressive_cube): 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: 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.on_ground_frame_counter = 0
self.crawl_start_potential = None self.crawl_start_potential = None
self.crawl_ignored_potential = 0.0 self.crawl_ignored_potential = 0.0

6
examples/pybullet/gym/pybullet_envs/robot_pendula.py
View File

@@ -6,7 +6,8 @@ class InvertedPendulum(MJCFBasedRobot):
def __init__(self): def __init__(self):
MJCFBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5) 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.pole = self.parts["pole"]
self.slider = self.jdict["slider"] self.slider = self.jdict["slider"]
self.j1 = self.jdict["hinge"] self.j1 = self.jdict["hinge"]
@@ -55,7 +56,8 @@ class InvertedDoublePendulum(MJCFBasedRobot):
def __init__(self): def __init__(self):
MJCFBasedRobot.__init__(self, 'inverted_double_pendulum.xml', 'cart', action_dim=1, obs_dim=9) 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.pole2 = self.parts["pole2"]
self.slider = self.jdict["slider"] self.slider = self.jdict["slider"]
self.j1 = self.jdict["hinge"] self.j1 = self.jdict["hinge"]

18
examples/pybullet/gym/pybullet_envs/scene_abstract.py
View File

@@ -1,6 +1,6 @@
import sys, os import sys, os
sys.path.append(os.path.dirname(__file__)) sys.path.append(os.path.dirname(__file__))
import pybullet as p import pybullet
import gym import gym
@@ -8,13 +8,14 @@ import gym
class Scene: class Scene:
"A base class for single- and multiplayer scenes" "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.np_random, seed = gym.utils.seeding.np_random(None)
self.timestep = timestep self.timestep = timestep
self.frame_skip = frame_skip self.frame_skip = frame_skip
self.dt = self.timestep * self.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.test_window_still_open = True # or never opened
self.human_render_detected = False # if user wants render("human"), we open test window self.human_render_detected = False # if user wants render("human"), we open test window
@@ -55,7 +56,8 @@ class SingleRobotEmptyScene(Scene):
class World: 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.gravity = gravity
self.timestep = timestep self.timestep = timestep
self.frame_skip = frame_skip self.frame_skip = frame_skip
@@ -65,12 +67,12 @@ class World:
def clean_everything(self): def clean_everything(self):
#p.resetSimulation() #p.resetSimulation()
p.setGravity(0, 0, -self.gravity) self._p.setGravity(0, 0, -self.gravity)
p.setDefaultContactERP(0.9) self._p.setDefaultContactERP(0.9)
#print("self.numSolverIterations=",self.numSolverIterations) #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): def step(self, frame_skip):
p.stepSimulation() self._p.stepSimulation()

18
examples/pybullet/gym/pybullet_envs/scene_stadium.py
View File

@@ -6,7 +6,7 @@ import pybullet_data
from pybullet_envs.scene_abstract import Scene from pybullet_envs.scene_abstract import Scene
import pybullet as p import pybullet
class StadiumScene(Scene): class StadiumScene(Scene):
@@ -15,8 +15,8 @@ class StadiumScene(Scene):
stadium_halfwidth = 50*0.25 # FOOBALL_FIELD_HALFWID stadium_halfwidth = 50*0.25 # FOOBALL_FIELD_HALFWID
stadiumLoaded=0 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() Scene.episode_restart(self) # contains cpp_world.clean_everything()
if (self.stadiumLoaded==0): if (self.stadiumLoaded==0):
self.stadiumLoaded=1 self.stadiumLoaded=1
@@ -26,17 +26,17 @@ class StadiumScene(Scene):
# stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants
filename = os.path.join(pybullet_data.getDataPath(),"plane_stadium.sdf") 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") #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: for i in self.ground_plane_mjcf:
p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) self._p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5)
p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) self._p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8])
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) self._p.configureDebugVisualizer(pybullet.COV_ENABLE_PLANAR_REFLECTION,1)
# for j in range(p.getNumJoints(i)): # 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 #despite the name (stadium_no_collision), it DID have collision, so don't add duplicate ground
class SinglePlayerStadiumScene(StadiumScene): class SinglePlayerStadiumScene(StadiumScene):