From 0818112ede6aa9f40a48d6484a0c184e3285ac6d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Jan 2019 16:21:52 -0800 Subject: [PATCH] fix some gym envs --- .../pybullet/gym/pybullet_envs/env_bases.py | 3 +- .../gym/pybullet_envs/examples/laikago.py | 80 +++++++++---------- .../gym/pybullet_envs/examples/testBike.py | 2 +- .../minitaur/envs/minitaur_gym_env_example.py | 7 +- .../minitaur_raibert_controller_example.py | 7 ++ .../envs/minitaur_reactive_env_example.py | 10 +++ .../pybullet/gym/pybullet_envs/robot_bases.py | 4 +- 7 files changed, 65 insertions(+), 48 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 24d903a7d..861363348 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -69,7 +69,7 @@ class MJCFBaseBulletEnv(gym.Env): self.potential = self.robot.calc_potential() return s - def render(self, mode='human'): + def render(self, mode='human', close=False): if mode == "human": self.isRender = True if mode != "rgb_array": @@ -125,7 +125,6 @@ class MJCFBaseBulletEnv(gym.Env): _render = render _reset = reset _seed = seed - _step = step class Camera: def __init__(self): diff --git a/examples/pybullet/gym/pybullet_envs/examples/laikago.py b/examples/pybullet/gym/pybullet_envs/examples/laikago.py index 9317d2085..b256f2a04 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/laikago.py +++ b/examples/pybullet/gym/pybullet_envs/examples/laikago.py @@ -10,23 +10,23 @@ plane = p.loadURDF("plane.urdf") p.setGravity(0,0,-9.8) p.setTimeStep(1./500) #p.setDefaultContactERP(0) -#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS +#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS urdfFlags = p.URDF_USE_SELF_COLLISION quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False) #enable collision between lower legs 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 lower_legs = [2,5,8,11] for l0 in lower_legs: - for l1 in lower_legs: - if (l1>l0): - enableCollision = 1 - 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) + for l1 in lower_legs: + if (l1>l0): + enableCollision = 1 + 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) jointIds=[] 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] for i in range (4): - jointOffsets.append(0) - jointOffsets.append(-0.7) - jointOffsets.append(0.7) + jointOffsets.append(0) + jointOffsets.append(-0.7) + jointOffsets.append(0.7) maxForceId = p.addUserDebugParameter("maxForce",0,100,20) @@ -50,35 +50,35 @@ for j in range (p.getNumJoints(quadruped)): if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): jointIds.append(j) - + p.getCameraImage(480,320) p.setRealTimeSimulation(0) joints=[] with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream: - for line in filestream: - print("line=",line) - maxForce = p.readUserDebugParameter(maxForceId) - currentline = line.split(",") - #print (currentline) - #print("-----") - frame = currentline[0] - t = currentline[1] - #print("frame[",frame,"]") - joints=currentline[2:14] - #print("joints=",joints) - for j in range (12): - targetPos = float(joints[j]) - p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) - p.stepSimulation() - for lower_leg in lower_legs: - #print("points for ", quadruped, " link: ", lower_leg) - pts = p.getContactPoints(quadruped,-1, lower_leg) - #print("num points=",len(pts)) - #for pt in pts: - # print(pt[9]) - time.sleep(1./500.) + for line in filestream: + print("line=",line) + maxForce = p.readUserDebugParameter(maxForceId) + currentline = line.split(",") + #print (currentline) + #print("-----") + frame = currentline[0] + t = currentline[1] + #print("frame[",frame,"]") + joints=currentline[2:14] + #print("joints=",joints) + for j in range (12): + targetPos = float(joints[j]) + p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) + p.stepSimulation() + for lower_leg in lower_legs: + #print("points for ", quadruped, " link: ", lower_leg) + pts = p.getContactPoints(quadruped,-1, lower_leg) + #print("num points=",len(pts)) + #for pt in pts: + # print(pt[9]) + time.sleep(1./500.) for j in range (p.getNumJoints(quadruped)): @@ -95,10 +95,10 @@ for j in range (p.getNumJoints(quadruped)): p.setRealTimeSimulation(1) while (1): - - for i in range(len(paramIds)): - c = paramIds[i] - targetPos = p.readUserDebugParameter(c) - maxForce = p.readUserDebugParameter(maxForceId) - p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce) - + + for i in range(len(paramIds)): + c = paramIds[i] + targetPos = p.readUserDebugParameter(c) + maxForce = p.readUserDebugParameter(maxForceId) + p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce) + diff --git a/examples/pybullet/gym/pybullet_envs/examples/testBike.py b/examples/pybullet/gym/pybullet_envs/examples/testBike.py index b62a8668e..e36922fdc 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/testBike.py +++ b/examples/pybullet/gym/pybullet_envs/examples/testBike.py @@ -24,7 +24,7 @@ for i in range (1): p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0) p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20) - p.changeDynamics(plane,-1, mass=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,2,lateralFriction=1,linearDamping=0, angularDamping=0) #p.resetJointState(bike,1,0,100) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py index 197560cfd..c05022648 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py @@ -4,10 +4,11 @@ import csv import math - -import os, inspect +import os +import inspect 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) import argparse diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py index 90a89a3ac..87958dde5 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py @@ -5,6 +5,13 @@ from __future__ import absolute_import from __future__ import division 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 from pybullet_envs.minitaur.envs import minitaur_raibert_controller from pybullet_envs.minitaur.envs import minitaur_gym_env diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py index 5057d9cf4..2c2be7106 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py @@ -6,11 +6,21 @@ from __future__ import print_function import os 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 from pybullet_envs.minitaur.agents.scripts import utility import pybullet_data from pybullet_envs.minitaur.envs import simple_ppo_agent + flags = tf.app.flags FLAGS = tf.app.flags.FLAGS LOG_DIR = os.path.join(pybullet_data.getDataPath(), "policies/ppo/minitaur_reactive_env") diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 40c104999..18fd3defe 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -22,9 +22,9 @@ class XmlBasedRobot: self.robot_body = None 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]) - 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.robot_name = robot_name