fix some gym envs

This commit is contained in:
Erwin Coumans
2019-01-28 16:21:52 -08:00
parent c97d1041ed
commit 0818112ede
7 changed files with 65 additions and 48 deletions

View File

@@ -69,7 +69,7 @@ class MJCFBaseBulletEnv(gym.Env):
self.potential = self.robot.calc_potential() self.potential = self.robot.calc_potential()
return s return s
def render(self, mode='human'): def render(self, mode='human', close=False):
if mode == "human": if mode == "human":
self.isRender = True self.isRender = True
if mode != "rgb_array": if mode != "rgb_array":
@@ -125,7 +125,6 @@ class MJCFBaseBulletEnv(gym.Env):
_render = render _render = render
_reset = reset _reset = reset
_seed = seed _seed = seed
_step = step
class Camera: class Camera:
def __init__(self): def __init__(self):

View File

@@ -17,16 +17,16 @@ quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = ur
#enable collision between lower legs #enable collision between lower legs
for j in range (p.getNumJoints(quadruped)): for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j)) print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs #2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11] lower_legs = [2,5,8,11]
for l0 in lower_legs: for l0 in lower_legs:
for l1 in lower_legs: for l1 in lower_legs:
if (l1>l0): if (l1>l0):
enableCollision = 1 enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision) print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision) p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[] jointIds=[]
paramIds=[] paramIds=[]
@@ -35,9 +35,9 @@ jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0] jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
for i in range (4): for i in range (4):
jointOffsets.append(0) jointOffsets.append(0)
jointOffsets.append(-0.7) jointOffsets.append(-0.7)
jointOffsets.append(0.7) jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce",0,100,20) maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
@@ -57,28 +57,28 @@ p.setRealTimeSimulation(0)
joints=[] joints=[]
with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream: with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream:
for line in filestream: for line in filestream:
print("line=",line) print("line=",line)
maxForce = p.readUserDebugParameter(maxForceId) maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",") currentline = line.split(",")
#print (currentline) #print (currentline)
#print("-----") #print("-----")
frame = currentline[0] frame = currentline[0]
t = currentline[1] t = currentline[1]
#print("frame[",frame,"]") #print("frame[",frame,"]")
joints=currentline[2:14] joints=currentline[2:14]
#print("joints=",joints) #print("joints=",joints)
for j in range (12): for j in range (12):
targetPos = float(joints[j]) targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation() p.stepSimulation()
for lower_leg in lower_legs: for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg) #print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg) pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts)) #print("num points=",len(pts))
#for pt in pts: #for pt in pts:
# print(pt[9]) # print(pt[9])
time.sleep(1./500.) time.sleep(1./500.)
for j in range (p.getNumJoints(quadruped)): for j in range (p.getNumJoints(quadruped)):
@@ -96,9 +96,9 @@ p.setRealTimeSimulation(1)
while (1): while (1):
for i in range(len(paramIds)): for i in range(len(paramIds)):
c = paramIds[i] c = paramIds[i]
targetPos = p.readUserDebugParameter(c) targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId) maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce) p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce)

View File

@@ -24,7 +24,7 @@ for i in range (1):
p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0) p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0)
p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20) p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20)
p.changeDynamics(plane,-1, mass=20,lateralFriction=1, linearDamping=0, angularDamping=0) p.changeDynamics(plane,-1, mass=0,lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0) p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0)
p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0) p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100) #p.resetJointState(bike,1,0,100)

View File

@@ -4,10 +4,11 @@
import csv import csv
import math import math
import os
import os, inspect 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(os.path.dirname(currentdir)))
print("parentdir=",parentdir)
os.sys.path.insert(0,parentdir) os.sys.path.insert(0,parentdir)
import argparse import argparse

View File

@@ -5,6 +5,13 @@ from __future__ import absolute_import
from __future__ import division from __future__ import division
from __future__ import print_function from __future__ import print_function
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir)))
os.sys.path.insert(0,parentdir)
import tensorflow as tf import tensorflow as tf
from pybullet_envs.minitaur.envs import minitaur_raibert_controller from pybullet_envs.minitaur.envs import minitaur_raibert_controller
from pybullet_envs.minitaur.envs import minitaur_gym_env from pybullet_envs.minitaur.envs import minitaur_gym_env

View File

@@ -6,11 +6,21 @@ from __future__ import print_function
import os import os
import time import time
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir)))
print("parentdir=",parentdir)
os.sys.path.insert(0,parentdir)
import tensorflow as tf import tensorflow as tf
from pybullet_envs.minitaur.agents.scripts import utility from pybullet_envs.minitaur.agents.scripts import utility
import pybullet_data import pybullet_data
from pybullet_envs.minitaur.envs import simple_ppo_agent from pybullet_envs.minitaur.envs import simple_ppo_agent
flags = tf.app.flags flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS FLAGS = tf.app.flags.FLAGS
LOG_DIR = os.path.join(pybullet_data.getDataPath(), "policies/ppo/minitaur_reactive_env") LOG_DIR = os.path.join(pybullet_data.getDataPath(), "policies/ppo/minitaur_reactive_env")

View File

@@ -22,9 +22,9 @@ class XmlBasedRobot:
self.robot_body = None self.robot_body = None
high = np.ones([action_dim]) high = np.ones([action_dim])
self.action_space = gym.spaces.Box(-high, high, dtype=np.float32) self.action_space = gym.spaces.Box(-high, high)
high = np.inf * np.ones([obs_dim]) high = np.inf * np.ones([obs_dim])
self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32) self.observation_space = gym.spaces.Box(-high, high)
#self.model_xml = model_xml #self.model_xml = model_xml
self.robot_name = robot_name self.robot_name = robot_name