fix some gym envs
This commit is contained in:
@@ -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):
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
Reference in New Issue
Block a user