diff options
6 files changed, 33 insertions, 12 deletions
diff --git a/examples/pybullet/gym/pybullet_data/stadium_no_collision.sdf b/examples/pybullet/gym/pybullet_data/stadium_no_collision.sdf index 585b4eb16..5ed0b533a 100644 --- a/examples/pybullet/gym/pybullet_data/stadium_no_collision.sdf +++ b/examples/pybullet/gym/pybullet_data/stadium_no_collision.sdf @@ -32,10 +32,10 @@ </visual> </link> </model> - <model name='roboschool/models_outdoor/stadium/part1.obj'> + <model name='floor_obj'> <static>1</static> <pose frame=''>0 0 0 0 0 0</pose> - <link name='link_d1'> + <link name='floor'> <inertial> <mass>0</mass> <inertia> diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py index 7fbf73585..ec97383da 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py @@ -73,7 +73,7 @@ def demo_run(): print("score=%0.2f in %i frames" % (score, frame)) if still_open!=True: # not True in multiplayer or non-Roboschool environment break - restart_delay = 123460*2 # 2 sec at 60 fps + restart_delay = 60*2 # 2 sec at 60 fps restart_delay -= 1 if restart_delay==0: break diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index f1a145956..41bb8321a 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -2,7 +2,7 @@ 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, HumanoidFlagrun +from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid, HumanoidFlagrun, HumanoidFlagrunHarder class WalkerBaseBulletEnv(MJCFBaseBulletEnv): @@ -160,10 +160,16 @@ class HumanoidFlagrunBulletEnv(HumanoidBulletEnv): s.zero_at_running_strip_start_line = False return s -class HumanoidFlagrunHarderBulletEnv(HumanoidFlagrunBulletEnv): +class HumanoidFlagrunHarderBulletEnv(HumanoidBulletEnv): random_lean = True # can fall on start def __init__(self): - HumanoidFlagrunBulletEnv.__init__(self) + self.robot = HumanoidFlagrunHarder() self.electricity_cost /= 4 # don't care that much about electricity, just stand up! + HumanoidBulletEnv.__init__(self, self.robot) + + def create_single_player_scene(self): + s = HumanoidBulletEnv.create_single_player_scene(self) + s.zero_at_running_strip_start_line = False + return s diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 989d22a54..d4059d2ac 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -245,6 +245,9 @@ class BodyPart: def current_orientation(self): return self.get_pose()[3:] + def get_orientation(self): + return self.current_orientation() + def reset_position(self, position): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation()) @@ -252,7 +255,7 @@ class BodyPart: p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) def reset_velocity(self, linearVelocity=[0,0,0], angularVelocity =[0,0,0]): - p.resetBaseVelocity(linearVelocity, angularVelocity) + p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity) def reset_pose(self, position, orientation): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation) @@ -299,6 +302,10 @@ class Joint: x, _ = self.get_state() return x + def get_orientation(self): + _,r = self.get_state() + return r + def get_velocity(self): _, vx = self.get_state() return vx diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index cf2866fa6..c8528eb6e 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -194,6 +194,7 @@ class Humanoid(WalkerBase): def get_cube(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 part_name, _ = p.getBodyInfo(body, 0) part_name = part_name.decode("utf8") bodies = [body] @@ -244,10 +245,15 @@ class HumanoidFlagrun(Humanoid): class HumanoidFlagrunHarder(HumanoidFlagrun): def __init__(self): - HumanoidFlagrun.__init__() + HumanoidFlagrun.__init__(self) + self.flag = None + self.aggressive_cube = None + self.frame = 0 def robot_specific_reset(self): HumanoidFlagrun.robot_specific_reset(self) + + self.frame = 0 if (self.aggressive_cube): p.resetBasePositionAndOrientation(self.aggressive_cube.bodies[0],[-1.5,0,0.05],[0,0,0,1]) else: @@ -279,6 +285,7 @@ class HumanoidFlagrunHarder(HumanoidFlagrun): elif self.on_ground_frame_counter > 0: self.on_ground_frame_counter -= 1 # End episode if the robot can't get up in 170 frames, to save computation and decorrelate observations. + self.frame += 1 return self.potential_leak() if self.on_ground_frame_counter<170 else -1 def potential_leak(self): diff --git a/examples/pybullet/gym/pybullet_envs/scene_stadium.py b/examples/pybullet/gym/pybullet_envs/scene_stadium.py index c68e628a6..d592e9116 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_stadium.py +++ b/examples/pybullet/gym/pybullet_envs/scene_stadium.py @@ -25,12 +25,13 @@ class StadiumScene(Scene): # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") - self.stadium = p.loadSDF(filename) - planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") + self.ground_plane_mjcf = p.loadSDF(filename) - self.ground_plane_mjcf = p.loadMJCF(planeName, flags=p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) for i in self.ground_plane_mjcf: - p.changeVisualShape(i,-1,rgbaColor=[0,0,0,0]) + p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) + for j in range(p.getNumJoints(i)): + p.changeDynamics(i,j,lateralFriction=0) + #despite the name (stadium_no_collision), it DID have collision, so don't add duplicate ground class SinglePlayerStadiumScene(StadiumScene): "This scene created by environment, to work in a way as if there was no concept of scene visible to user." |