Merge pull request #1279 from erwincoumans/master
more fixes in the pybullet gym environments
This commit is contained in:
15
.gitignore
vendored
15
.gitignore
vendored
@@ -3,3 +3,18 @@
|
||||
/build_cmake/
|
||||
|
||||
*.pyc
|
||||
|
||||
# Python
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
|
||||
# Pip
|
||||
pip-selfcheck.json
|
||||
*.whl
|
||||
*.egg
|
||||
*.egg-info
|
||||
|
||||
# Setuptools
|
||||
/build
|
||||
/dist
|
||||
*.eggs
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
import os
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
@@ -52,15 +53,15 @@ class KukaGymEnv(gym.Env):
|
||||
p.resetSimulation()
|
||||
p.setPhysicsEngineParameter(numSolverIterations=150)
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
|
||||
p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"),[0,0,-1])
|
||||
|
||||
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
|
||||
p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
|
||||
|
||||
xpos = 0.5 +0.05*random.random()
|
||||
ypos = 0 +0.05*random.random()
|
||||
ang = 3.1415925438*random.random()
|
||||
orn = p.getQuaternionFromEuler([0,0,ang])
|
||||
self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
|
||||
self.blockUid =p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
|
||||
|
||||
@@ -15,11 +15,11 @@ class MJCFBaseBulletEnv(gym.Env):
|
||||
'video.frames_per_second': 60
|
||||
}
|
||||
|
||||
def __init__(self, robot):
|
||||
def __init__(self, robot, render=False):
|
||||
self.scene = None
|
||||
|
||||
self.physicsClientId=-1
|
||||
self.camera = Camera()
|
||||
|
||||
self.isRender = render
|
||||
self.robot = robot
|
||||
|
||||
self._seed()
|
||||
@@ -33,6 +33,15 @@ class MJCFBaseBulletEnv(gym.Env):
|
||||
return [seed]
|
||||
|
||||
def _reset(self):
|
||||
print("self.isRender=")
|
||||
print(self.isRender)
|
||||
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)
|
||||
|
||||
if self.scene is None:
|
||||
self.scene = self.create_single_player_scene()
|
||||
if not self.scene.multiplayer:
|
||||
@@ -49,7 +58,13 @@ class MJCFBaseBulletEnv(gym.Env):
|
||||
return s
|
||||
|
||||
def _render(self, mode, close):
|
||||
pass
|
||||
if (mode=="human"):
|
||||
self.isRender = True
|
||||
|
||||
def _close(self):
|
||||
if (self.physicsClientId>=0):
|
||||
p.disconnect(self.physicsClientId)
|
||||
self.physicsClientId = -1
|
||||
|
||||
def HUD(self, state, a, done):
|
||||
pass
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
from . import enjoy_TF_AntBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_HalfCheetahBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_AntBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_HopperBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_HumanoidBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_InvertedPendulumBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may
|
||||
from . import enjoy_TF_Walker2DBulletEnv_v0_2017may
|
||||
from . import enjoy_kuka_grasping
|
||||
from . import enjoy_pybullet_cartpole
|
||||
from . import enjoy_pybullet_racecar
|
||||
from . import enjoy_pybullet_zed_racecar
|
||||
from . import kukaGymEnvTest
|
||||
from . import racecarGymEnvTest
|
||||
from . import racecarZEDGymEnvTest
|
||||
from . import simpleHumanoidGymEnvTest
|
||||
from . import train_kuka_cam_grasping
|
||||
from . import train_kuka_grasping
|
||||
from . import train_pybullet_cartpole
|
||||
from . import train_pybullet_racecar
|
||||
from . import train_pybullet_zed_racecar
|
||||
|
||||
|
||||
@@ -28,16 +28,12 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("AntBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
env.render(mode="human")
|
||||
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
torsoId = -1
|
||||
for i in range (p.getNumBodies()):
|
||||
print(p.getBodyInfo(i))
|
||||
@@ -49,10 +45,8 @@ def demo_run():
|
||||
frame = 0
|
||||
score = 0
|
||||
restart_delay = 0
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
obs = env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
|
||||
|
||||
while 1:
|
||||
time.sleep(0.001)
|
||||
a = pi.act(obs)
|
||||
@@ -311,4 +305,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ -0.0680, +0.1401, -0.0628, -0.1317, +0.1489, +0.1844, -0.1147, +0.0137])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -27,17 +27,13 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("HalfCheetahBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
env.render(mode="human")
|
||||
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
#disable rendering during reset, makes loading much faster
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
torsoId = -1
|
||||
for i in range (p.getNumBodies()):
|
||||
print(p.getBodyInfo(i))
|
||||
@@ -52,10 +48,8 @@ def demo_run():
|
||||
frame = 0
|
||||
score = 0
|
||||
restart_delay = 0
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
obs = env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
|
||||
|
||||
while 1:
|
||||
time.sleep(0.001)
|
||||
a = pi.act(obs)
|
||||
@@ -311,4 +305,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ +0.1367, +0.1107, -0.0148, +0.1158, -0.0820, +0.3047])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -30,15 +30,12 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("HopperBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
env.render(mode="human")
|
||||
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
for i in range (p.getNumBodies()):
|
||||
print(p.getBodyInfo(i))
|
||||
if (p.getBodyInfo(i)[1].decode() == "hopper"):
|
||||
@@ -52,10 +49,8 @@ def demo_run():
|
||||
score = 0
|
||||
restart_delay = 0
|
||||
#disable rendering during reset, makes loading much faster
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
obs = env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
|
||||
|
||||
while 1:
|
||||
time.sleep(0.001)
|
||||
a = pi.act(obs)
|
||||
@@ -301,4 +296,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ +0.4868, -0.0987, -0.0946])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -28,15 +28,11 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("HumanoidBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
env.render(mode="human")
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
torsoId = -1
|
||||
for i in range (p.getNumBodies()):
|
||||
print(p.getBodyInfo(i))
|
||||
@@ -47,10 +43,8 @@ def demo_run():
|
||||
frame = 0
|
||||
score = 0
|
||||
restart_delay = 0
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
obs = env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
|
||||
|
||||
while 1:
|
||||
time.sleep(0.001)
|
||||
a = pi.act(obs)
|
||||
@@ -517,4 +511,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ -0.0356, +0.0776, -0.0344, +0.1375, +0.1048, +0.3648, +0.3240, +0.1319, +0.1161, +0.3373, +0.3193, +0.0120, +0.0253, -0.2434, -0.1291, +0.1042, -0.2448])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -27,11 +27,10 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("InvertedDoublePendulumBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
|
||||
env.render(mode="human")
|
||||
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
|
||||
while 1:
|
||||
@@ -179,4 +178,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ +0.0190])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -27,11 +27,10 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
print("create env")
|
||||
env = gym.make("InvertedPendulumBulletEnv-v0")
|
||||
print("connecting")
|
||||
cid = p.connect(p.GUI)
|
||||
env.render(mode="human")
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
|
||||
while 1:
|
||||
@@ -174,4 +173,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ +0.0352])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -27,11 +27,10 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("InvertedPendulumSwingupBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
|
||||
env.render(mode="human")
|
||||
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
|
||||
while 1:
|
||||
@@ -175,4 +174,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ +0.2753])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -27,17 +27,12 @@ class SmallReactivePolicy:
|
||||
x = np.dot(x, weights_final_w) + weights_final_b
|
||||
return x
|
||||
|
||||
def demo_run():
|
||||
def main():
|
||||
env = gym.make("Walker2DBulletEnv-v0")
|
||||
|
||||
cid = p.connect(p.GUI)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
env.render(mode="human")
|
||||
pi = SmallReactivePolicy(env.observation_space, env.action_space)
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
env.reset()
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
torsoId = -1
|
||||
for i in range (p.getNumBodies()):
|
||||
print(p.getBodyInfo(i))
|
||||
@@ -305,4 +300,4 @@ weights_final_w = np.array([
|
||||
weights_final_b = np.array([ +0.0090, +0.2404, +0.1022, +0.1035, +0.0621, -0.0160])
|
||||
|
||||
if __name__=="__main__":
|
||||
demo_run()
|
||||
main()
|
||||
|
||||
@@ -7,31 +7,34 @@ os.sys.path.insert(0,parentdir)
|
||||
from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv
|
||||
import time
|
||||
|
||||
def main():
|
||||
|
||||
environment = KukaGymEnv(renders=True)
|
||||
|
||||
|
||||
motorsIds=[]
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
|
||||
|
||||
dv = 0.001
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
|
||||
|
||||
done = False
|
||||
while (not done):
|
||||
|
||||
action=[]
|
||||
for motorId in motorsIds:
|
||||
action.append(environment._p.readUserDebugParameter(motorId))
|
||||
|
||||
state, reward, done, info = environment.step2(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
|
||||
environment = KukaGymEnv(renders=True)
|
||||
|
||||
|
||||
motorsIds=[]
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
|
||||
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
|
||||
|
||||
dv = 0.001
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
|
||||
|
||||
done = False
|
||||
while (not done):
|
||||
|
||||
action=[]
|
||||
for motorId in motorsIds:
|
||||
action.append(environment._p.readUserDebugParameter(motorId))
|
||||
|
||||
state, reward, done, info = environment.step2(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
|
||||
if __name__=="__main__":
|
||||
main()
|
||||
@@ -7,33 +7,38 @@ os.sys.path.insert(0,parentdir)
|
||||
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
|
||||
isDiscrete = False
|
||||
|
||||
environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete)
|
||||
environment.reset()
|
||||
def main():
|
||||
|
||||
environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete)
|
||||
environment.reset()
|
||||
|
||||
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
|
||||
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
|
||||
|
||||
while (True):
|
||||
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
|
||||
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
|
||||
if (isDiscrete):
|
||||
discreteAction = 0
|
||||
if (targetVelocity<-0.33):
|
||||
discreteAction=0
|
||||
else:
|
||||
if (targetVelocity>0.33):
|
||||
discreteAction=6
|
||||
else:
|
||||
discreteAction=3
|
||||
if (steeringAngle>-0.17):
|
||||
if (steeringAngle>0.17):
|
||||
discreteAction=discreteAction+2
|
||||
else:
|
||||
discreteAction=discreteAction+1
|
||||
action=discreteAction
|
||||
else:
|
||||
action=[targetVelocity,steeringAngle]
|
||||
state, reward, done, info = environment.step(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
print("obs")
|
||||
print(obs)
|
||||
|
||||
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
|
||||
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
|
||||
|
||||
while (True):
|
||||
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
|
||||
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
|
||||
if (isDiscrete):
|
||||
discreteAction = 0
|
||||
if (targetVelocity<-0.33):
|
||||
discreteAction=0
|
||||
else:
|
||||
if (targetVelocity>0.33):
|
||||
discreteAction=6
|
||||
else:
|
||||
discreteAction=3
|
||||
if (steeringAngle>-0.17):
|
||||
if (steeringAngle>0.17):
|
||||
discreteAction=discreteAction+2
|
||||
else:
|
||||
discreteAction=discreteAction+1
|
||||
action=discreteAction
|
||||
else:
|
||||
action=[targetVelocity,steeringAngle]
|
||||
state, reward, done, info = environment.step(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
print("obs")
|
||||
print(obs)
|
||||
if __name__=="__main__":
|
||||
main()
|
||||
@@ -5,31 +5,36 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
|
||||
os.sys.path.insert(0,parentdir)
|
||||
|
||||
from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
|
||||
print ("hello")
|
||||
environment = RacecarZEDGymEnv(renders=True)
|
||||
|
||||
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
|
||||
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
|
||||
def main():
|
||||
|
||||
environment = RacecarZEDGymEnv(renders=True)
|
||||
|
||||
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
|
||||
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
|
||||
|
||||
while (True):
|
||||
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
|
||||
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
|
||||
discreteAction = 0
|
||||
if (targetVelocity<-0.33):
|
||||
discreteAction=0
|
||||
else:
|
||||
if (targetVelocity>0.33):
|
||||
discreteAction=6
|
||||
else:
|
||||
discreteAction=3
|
||||
if (steeringAngle>-0.17):
|
||||
if (steeringAngle>0.17):
|
||||
discreteAction=discreteAction+2
|
||||
else:
|
||||
discreteAction=discreteAction+1
|
||||
action=discreteAction
|
||||
|
||||
state, reward, done, info = environment.step(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
print("obs")
|
||||
print(obs)
|
||||
|
||||
while (True):
|
||||
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
|
||||
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
|
||||
discreteAction = 0
|
||||
if (targetVelocity<-0.33):
|
||||
discreteAction=0
|
||||
else:
|
||||
if (targetVelocity>0.33):
|
||||
discreteAction=6
|
||||
else:
|
||||
discreteAction=3
|
||||
if (steeringAngle>-0.17):
|
||||
if (steeringAngle>0.17):
|
||||
discreteAction=discreteAction+2
|
||||
else:
|
||||
discreteAction=discreteAction+1
|
||||
action=discreteAction
|
||||
|
||||
state, reward, done, info = environment.step(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
print("obs")
|
||||
print(obs)
|
||||
if __name__=="__main__":
|
||||
main()
|
||||
@@ -5,22 +5,27 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
|
||||
os.sys.path.insert(0,parentdir)
|
||||
|
||||
from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv
|
||||
print ("hello")
|
||||
environment = SimpleHumanoidGymEnv(renders=True)
|
||||
|
||||
environment._p.setGravity(0,0,0)
|
||||
def main():
|
||||
|
||||
environment = SimpleHumanoidGymEnv(renders=True)
|
||||
|
||||
motorsIds=[]
|
||||
for motor in environment._humanoid.motor_names:
|
||||
motorsIds.append(environment._p.addUserDebugParameter(motor,-1,1,0))
|
||||
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)
|
||||
|
||||
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()
|
||||
@@ -1,12 +1,15 @@
|
||||
from .scene_stadium import SinglePlayerStadiumScene
|
||||
from .env_bases import MJCFBaseBulletEnv
|
||||
import numpy as np
|
||||
import pybullet as p
|
||||
from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid
|
||||
|
||||
|
||||
class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
|
||||
def __init__(self, robot):
|
||||
MJCFBaseBulletEnv.__init__(self, robot)
|
||||
def __init__(self, robot, render=False):
|
||||
print("WalkerBase::__init__")
|
||||
MJCFBaseBulletEnv.__init__(self, robot, render)
|
||||
|
||||
self.camera_x = 0
|
||||
self.walk_target_x = 1e3 # kilometer away
|
||||
self.walk_target_y = 0
|
||||
@@ -16,11 +19,15 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
|
||||
return self.stadium_scene
|
||||
|
||||
def _reset(self):
|
||||
|
||||
r = MJCFBaseBulletEnv._reset(self)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
|
||||
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(
|
||||
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)
|
||||
return r
|
||||
|
||||
def move_robot(self, init_x, init_y, init_z):
|
||||
|
||||
4
setup.py
4
setup.py
@@ -428,7 +428,7 @@ hh = setup_py_dir + "/" + datadir
|
||||
for root, dirs, files in os.walk(hh):
|
||||
for fn in files:
|
||||
ext = os.path.splitext(fn)[1][1:]
|
||||
if ext and ext in 'png jpg urdf sdf obj mtl dae off stl STL xml '.split():
|
||||
if ext and ext in 'png gif jpg urdf sdf obj mtl dae off stl STL xml '.split():
|
||||
fn = root + "/" + fn
|
||||
need_files.append(fn[1+len(hh):])
|
||||
|
||||
@@ -440,7 +440,7 @@ print("-----")
|
||||
|
||||
setup(
|
||||
name = 'pybullet',
|
||||
version='1.2.4',
|
||||
version='1.2.6',
|
||||
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
|
||||
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
|
||||
url='https://github.com/bulletphysics/bullet3',
|
||||
|
||||
Reference in New Issue
Block a user