dodox/fancy_gym
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

current state

Browse Source
This commit is contained in:
Fabian
2022-06-30 17:33:05 +02:00
parent 60bdeef687
commit fea2ae7d11
52 changed files with 325 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files
alr_envs/alr/mujoco/hopper_jump/__init__.py
+1 -1
View File
@@ -1 +1 @@
from .new_mp_wrapper import MPWrapper
from .mp_wrapper import MPWrapper
alr_envs/alr/mujoco/hopper_jump/hopper_jump.py
+70 -77
View File
@@ -1,3 +1,5 @@
from typing import Optional
from gym.envs.mujoco.hopper_v3 import HopperEnv
import numpy as np
import os
@@ -8,10 +10,10 @@ MAX_EPISODE_STEPS_HOPPERJUMP = 250
class ALRHopperJumpEnv(HopperEnv):
"""
Initialization changes to normal Hopper:
- healthy_reward: 1.0 -> 0.1 -> 0
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
- terminate_when_unhealthy: True -> False
- healthy_z_range: (0.7, float('inf')) -> (0.5, float('inf'))
- exclude current positions from observatiosn is set to False
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
- exclude_current_positions_from_observation: True -> False
"""
def __init__(
@@ -19,76 +21,93 @@ class ALRHopperJumpEnv(HopperEnv):
xml_file='hopper_jump.xml',
forward_reward_weight=1.0,
ctrl_cost_weight=1e-3,
healthy_reward=0.0,
healthy_reward=1.0,
penalty=0.0,
context=True,
terminate_when_unhealthy=False,
healthy_state_range=(-100.0, 100.0),
healthy_z_range=(0.5, float('inf')),
healthy_angle_range=(-float('inf'), float('inf')),
reset_noise_scale=5e-3,
exclude_current_positions_from_observation=False,
max_episode_steps=250
):
):
self.current_step = 0
self._steps = 0
self.max_height = 0
self.max_episode_steps = max_episode_steps
self.penalty = penalty
# self.penalty = penalty
self.goal = 0
self.context = context
self.exclude_current_positions_from_observation = exclude_current_positions_from_observation
self._floor_geom_id = None
self._foot_geom_id = None
self.contact_with_floor = False
self.init_floor_contact = False
self.has_left_floor = False
self.contact_dist = None
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
exclude_current_positions_from_observation)
def step(self, action):
self._steps += 1
self._floor_geom_id = self.model.geom_name2id('floor')
self._foot_geom_id = self.model.geom_name2id('foot_geom')
self.current_step += 1
self.do_simulation(action, self.frame_skip)
height_after = self.get_body_com("torso")[2]
# site_pos_after = self.sim.data.site_xpos[self.model.site_name2id('foot_site')].copy()
site_pos_after = self.get_body_com('foot_site')
site_pos_after = self.data.get_site_xpos('foot_site')
self.max_height = max(height_after, self.max_height)
has_floor_contact = self._is_floor_foot_contact() if not self.contact_with_floor else False
if not self.init_floor_contact:
self.init_floor_contact = has_floor_contact
if self.init_floor_contact and not self.has_left_floor:
self.has_left_floor = not has_floor_contact
if not self.contact_with_floor and self.has_left_floor:
self.contact_with_floor = has_floor_contact
ctrl_cost = self.control_cost(action)
costs = ctrl_cost
done = False
goal_dist = np.linalg.norm(site_pos_after - np.array([self.goal, 0, 0]))
if self.contact_dist is None and self.contact_with_floor:
self.contact_dist = goal_dist
rewards = 0
if self.current_step >= self.max_episode_steps:
hight_goal_distance = -10 * np.linalg.norm(self.max_height - self.goal) if self.context else self.max_height
healthy_reward = 0 if self.context else self.healthy_reward * 2 # self.current_step
height_reward = self._forward_reward_weight * hight_goal_distance # maybe move reward calculation into if structure and define two different _forward_reward_weight variables for context and episodic seperatley
rewards = height_reward + healthy_reward
if self._steps >= MAX_EPISODE_STEPS_HOPPERJUMP:
# healthy_reward = 0 if self.context else self.healthy_reward * self._steps
healthy_reward = self.healthy_reward * 2 # * self._steps
contact_dist = self.contact_dist if self.contact_dist is not None else 5
dist_reward = self._forward_reward_weight * (-3 * goal_dist + 10 * self.max_height - 2 * contact_dist)
rewards = dist_reward + healthy_reward
observation = self._get_obs()
reward = rewards - costs
info = {
'height': height_after,
'x_pos': site_pos_after,
'max_height': self.max_height,
'height_rew': self.max_height,
'healthy_reward': self.healthy_reward * 2,
'healthy': self.is_healthy
}
info = dict(
height=height_after,
x_pos=site_pos_after,
max_height=self.max_height,
goal=self.goal,
goal_dist=goal_dist,
height_rew=self.max_height,
healthy_reward=self.healthy_reward * 2,
healthy=self.is_healthy,
contact_dist=self.contact_dist if self.contact_dist is not None else 0
)
return observation, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.goal)
def reset(self):
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None, ):
self.goal = self.np_random.uniform(1.4, 2.16, 1)[0] # 1.3 2.3
self.max_height = 0
self.current_step = 0
self._steps = 0
return super().reset()
# overwrite reset_model to make it deterministic
@@ -106,11 +125,13 @@ class ALRHopperJumpEnv(HopperEnv):
self.contact_dist = None
return observation
def _contact_checker(self, id_1, id_2):
for coni in range(0, self.sim.data.ncon):
con = self.sim.data.contact[coni]
collision = con.geom1 == id_1 and con.geom2 == id_2
collision_trans = con.geom1 == id_2 and con.geom2 == id_1
def _is_floor_foot_contact(self):
floor_geom_id = self.model.geom_name2id('floor')
foot_geom_id = self.model.geom_name2id('foot_geom')
for i in range(self.data.ncon):
contact = self.data.contact[i]
collision = contact.geom1 == floor_geom_id and contact.geom2 == foot_geom_id
collision_trans = contact.geom1 == foot_geom_id and contact.geom2 == floor_geom_id
if collision or collision_trans:
return True
return False
@@ -122,7 +143,7 @@ class ALRHopperXYJumpEnv(ALRHopperJumpEnv):
self._floor_geom_id = self.model.geom_name2id('floor')
self._foot_geom_id = self.model.geom_name2id('foot_geom')
self.current_step += 1
self._steps += 1
self.do_simulation(action, self.frame_skip)
height_after = self.get_body_com("torso")[2]
site_pos_after = self.sim.data.site_xpos[self.model.site_name2id('foot_site')].copy()
@@ -133,8 +154,8 @@ class ALRHopperXYJumpEnv(ALRHopperJumpEnv):
# self.has_left_floor = not floor_contact if self.init_floor_contact and not self.has_left_floor else self.has_left_floor
# self.contact_with_floor = floor_contact if not self.contact_with_floor and self.has_left_floor else self.contact_with_floor
floor_contact = self._contact_checker(self._floor_geom_id,
self._foot_geom_id) if not self.contact_with_floor else False
floor_contact = self._is_floor_foot_contact(self._floor_geom_id,
self._foot_geom_id) if not self.contact_with_floor else False
if not self.init_floor_contact:
self.init_floor_contact = floor_contact
if self.init_floor_contact and not self.has_left_floor:
@@ -151,9 +172,9 @@ class ALRHopperXYJumpEnv(ALRHopperJumpEnv):
done = False
goal_dist = np.linalg.norm(site_pos_after - np.array([self.goal, 0, 0]))
rewards = 0
if self.current_step >= self.max_episode_steps:
# healthy_reward = 0 if self.context else self.healthy_reward * self.current_step
healthy_reward = self.healthy_reward * 2 # * self.current_step
if self._steps >= self.max_episode_steps:
# healthy_reward = 0 if self.context else self.healthy_reward * self._steps
healthy_reward = self.healthy_reward * 2 # * self._steps
contact_dist = self.contact_dist if self.contact_dist is not None else 5
dist_reward = self._forward_reward_weight * (-3 * goal_dist + 10 * self.max_height - 2 * contact_dist)
rewards = dist_reward + healthy_reward
@@ -170,7 +191,7 @@ class ALRHopperXYJumpEnv(ALRHopperJumpEnv):
'healthy_reward': self.healthy_reward * 2,
'healthy': self.is_healthy,
'contact_dist': self.contact_dist if self.contact_dist is not None else 0
}
}
return observation, reward, done, info
def reset_model(self):
@@ -242,7 +263,7 @@ class ALRHopperXYJumpEnvStepBased(ALRHopperXYJumpEnv):
height_scale=10,
dist_scale=3,
healthy_scale=2
):
):
self.height_scale = height_scale
self.dist_scale = dist_scale
self.healthy_scale = healthy_scale
@@ -254,7 +275,7 @@ class ALRHopperXYJumpEnvStepBased(ALRHopperXYJumpEnv):
self._floor_geom_id = self.model.geom_name2id('floor')
self._foot_geom_id = self.model.geom_name2id('foot_geom')
self.current_step += 1
self._steps += 1
self.do_simulation(action, self.frame_skip)
height_after = self.get_body_com("torso")[2]
site_pos_after = self.sim.data.site_xpos[self.model.site_name2id('foot_site')].copy()
@@ -273,8 +294,8 @@ class ALRHopperXYJumpEnvStepBased(ALRHopperXYJumpEnv):
###########################################################
# This is only for logging the distance to goal when first having the contact
##########################################################
floor_contact = self._contact_checker(self._floor_geom_id,
self._foot_geom_id) if not self.contact_with_floor else False
floor_contact = self._is_floor_foot_contact(self._floor_geom_id,
self._foot_geom_id) if not self.contact_with_floor else False
if not self.init_floor_contact:
self.init_floor_contact = floor_contact
if self.init_floor_contact and not self.has_left_floor:
@@ -295,33 +316,5 @@ class ALRHopperXYJumpEnvStepBased(ALRHopperXYJumpEnv):
'healthy_reward': self.healthy_reward * self.healthy_reward,
'healthy': self.is_healthy,
'contact_dist': self.contact_dist if self.contact_dist is not None else 0
}
}
return observation, reward, done, info
if __name__ == '__main__':
render_mode = "human" # "human" or "partial" or "final"
# env = ALRHopperJumpEnv()
# env = ALRHopperXYJumpEnv()
np.random.seed(0)
env = ALRHopperXYJumpEnvStepBased()
env.seed(0)
# env = ALRHopperJumpRndmPosEnv()
obs = env.reset()
for k in range(1000):
obs = env.reset()
print('observation :', obs[:])
for i in range(200):
# objective.load_result("/tmp/cma")
# test with random actions
ac = env.action_space.sample()
obs, rew, d, info = env.step(ac)
# if i % 10 == 0:
# env.render(mode=render_mode)
env.render(mode=render_mode)
if d:
print('After ', i, ' steps, done: ', d)
env.reset()
env.close()
alr_envs/alr/mujoco/hopper_jump/mp_wrapper.py
+10 -42
View File
@@ -1,57 +1,25 @@
from typing import Tuple, Union
from typing import Union, Tuple
import numpy as np
from alr_envs.mp.raw_interface_wrapper import RawInterfaceWrapper
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
@property
def context_mask(self) -> np.ndarray:
# Random x goal + random init pos
def context_mask(self):
return np.hstack([
[False] * (5 + int(not self.exclude_current_positions_from_observation)), # position
[False] * (2 + int(not self.exclude_current_positions_from_observation)), # position
[True] * 3, # set to true if randomize initial pos
[False] * 6, # velocity
[True]
])
@property
def current_pos(self) -> Union[float, int, np.ndarray]:
return self.env.sim.data.qpos[3:6].copy()
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
return self.sim.data.qpos[3:6].copy()
@property
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
return self.env.sim.data.qvel[3:6].copy()
@property
def goal_pos(self) -> Union[float, int, np.ndarray, Tuple]:
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
@property
def dt(self) -> Union[float, int]:
return self.env.dt
class HighCtxtMPWrapper(MPWrapper):
@property
def active_obs(self):
return np.hstack([
[True] * (5 + int(not self.exclude_current_positions_from_observation)), # position
[False] * 6, # velocity
[False]
])
@property
def current_pos(self) -> Union[float, int, np.ndarray]:
return self.env.sim.data.qpos[3:6].copy()
@property
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
return self.env.sim.data.qvel[3:6].copy()
@property
def goal_pos(self) -> Union[float, int, np.ndarray, Tuple]:
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
@property
def dt(self) -> Union[float, int]:
return self.env.dt
return self.sim.data.qvel[3:6].copy()
alr_envs/alr/mujoco/hopper_jump/new_mp_wrapper.py
-45
View File
@@ -1,45 +0,0 @@
from alr_envs.mp.black_box_wrapper import BlackBoxWrapper
from typing import Union, Tuple
import numpy as np
class MPWrapper(BlackBoxWrapper):
@property
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
return self.env.sim.data.qpos[3:6].copy()
@property
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
return self.env.sim.data.qvel[3:6].copy()
# # random goal
# def set_active_obs(self):
# return np.hstack([
# [False] * (5 + int(not self.env.exclude_current_positions_from_observation)), # position
# [False] * 6, # velocity
# [True]
# ])
# Random x goal + random init pos
def get_context_mask(self):
return np.hstack([
[False] * (2 + int(not self.env.exclude_current_positions_from_observation)), # position
[True] * 3, # set to true if randomize initial pos
[False] * 6, # velocity
[True]
])
class NewHighCtxtMPWrapper(MPWrapper):
def get_context_mask(self):
return np.hstack([
[False] * (2 + int(not self.env.exclude_current_positions_from_observation)), # position
[True] * 3, # set to true if randomize initial pos
[False] * 6, # velocity
[True], # goal
[False] * 3 # goal diff
])
def set_context(self, context):
return self.get_observation_from_step(self.env.env.set_context(context))