From d4e3b957a9693ee64cd6e1fc0b363a13dc291692 Mon Sep 17 00:00:00 2001 From: Onur Date: Fri, 1 Jul 2022 09:54:42 +0200 Subject: [PATCH] finish up beerpong, walker2d and ant needs more extensions, fix import bugs. --- alr_envs/alr/__init__.py | 11 +++- .../hole_reacher/mp_wrapper.py | 2 +- alr_envs/alr/mujoco/__init__.py | 1 - alr_envs/alr/mujoco/ant_jump/ant_jump.py | 25 ++++---- alr_envs/alr/mujoco/beerpong/beerpong.py | 57 +++++-------------- .../mujoco/beerpong/beerpong_reward_staged.py | 16 +++--- .../mujoco/walker_2d_jump/assets/walker2d.xml | 4 +- .../mujoco/walker_2d_jump/walker_2d_jump.py | 28 ++++----- alr_envs/utils/make_env_helpers.py | 12 ++-- 9 files changed, 62 insertions(+), 94 deletions(-) diff --git a/alr_envs/alr/__init__.py b/alr_envs/alr/__init__.py index 3d2415c..459a0e6 100644 --- a/alr_envs/alr/__init__.py +++ b/alr_envs/alr/__init__.py @@ -218,8 +218,15 @@ register( entry_point='alr_envs.alr.mujoco:ALRBeerBongEnv', max_episode_steps=300, kwargs={ - "rndm_goal": True, - "cup_goal_pos": [-0.3, -1.2], + "frame_skip": 2 + } +) + +register( + id='ALRBeerPong-v1', + entry_point='alr_envs.alr.mujoco:ALRBeerBongEnv', + max_episode_steps=300, + kwargs={ "frame_skip": 2 } ) diff --git a/alr_envs/alr/classic_control/hole_reacher/mp_wrapper.py b/alr_envs/alr/classic_control/hole_reacher/mp_wrapper.py index 85bc784..0a5eaa6 100644 --- a/alr_envs/alr/classic_control/hole_reacher/mp_wrapper.py +++ b/alr_envs/alr/classic_control/hole_reacher/mp_wrapper.py @@ -2,7 +2,7 @@ from typing import Tuple, Union 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): diff --git a/alr_envs/alr/mujoco/__init__.py b/alr_envs/alr/mujoco/__init__.py index df52cfc..359f97b 100644 --- a/alr_envs/alr/mujoco/__init__.py +++ b/alr_envs/alr/mujoco/__init__.py @@ -7,6 +7,5 @@ from .hopper_jump.hopper_jump_on_box import ALRHopperJumpOnBoxEnv from .hopper_throw.hopper_throw import ALRHopperThrowEnv from .hopper_throw.hopper_throw_in_basket import ALRHopperThrowInBasketEnv from .reacher.reacher import ReacherEnv -from .reacher.balancing import BalancingEnv from .table_tennis.tt_gym import TTEnvGym from .walker_2d_jump.walker_2d_jump import ALRWalker2dJumpEnv diff --git a/alr_envs/alr/mujoco/ant_jump/ant_jump.py b/alr_envs/alr/mujoco/ant_jump/ant_jump.py index 09c623d..27098ac 100644 --- a/alr_envs/alr/mujoco/ant_jump/ant_jump.py +++ b/alr_envs/alr/mujoco/ant_jump/ant_jump.py @@ -2,6 +2,10 @@ import numpy as np from gym.envs.mujoco.ant_v3 import AntEnv MAX_EPISODE_STEPS_ANTJUMP = 200 +# TODO: This environment was not testet yet. Do the following todos and test it. +# TODO: Right now this environment only considers jumping to a specific height, which is not nice. It should be extended +# to the same structure as the Hopper, where the angles are randomized (->contexts) and the agent should jump as heigh +# as possible, while landing at a specific target position class ALRAntJumpEnv(AntEnv): @@ -22,14 +26,12 @@ class ALRAntJumpEnv(AntEnv): healthy_z_range=(0.3, float('inf')), contact_force_range=(-1.0, 1.0), reset_noise_scale=0.1, - context=True, # variable to decide if context is used or not exclude_current_positions_from_observation=True, max_episode_steps=200): self.current_step = 0 self.max_height = 0 - self.context = context self.max_episode_steps = max_episode_steps - self.goal = 0 # goal when training with context + self.goal = 0 super().__init__(xml_file, ctrl_cost_weight, contact_cost_weight, healthy_reward, terminate_when_unhealthy, healthy_z_range, contact_force_range, reset_noise_scale, exclude_current_positions_from_observation) @@ -53,15 +55,11 @@ class ALRAntJumpEnv(AntEnv): done = height < 0.3 # fall over -> is the 0.3 value from healthy_z_range? TODO change 0.3 to the value of healthy z angle if self.current_step == self.max_episode_steps or done: - if self.context: - # -10 for scaling the value of the distance between the max_height and the goal height; only used when context is enabled - # height_reward = -10 * (np.linalg.norm(self.max_height - self.goal)) - height_reward = -10*np.linalg.norm(self.max_height - self.goal) - # no healthy reward when using context, because we optimize a negative value - healthy_reward = 0 - else: - height_reward = self.max_height - 0.7 - healthy_reward = self.healthy_reward * self.current_step + # -10 for scaling the value of the distance between the max_height and the goal height; only used when context is enabled + # height_reward = -10 * (np.linalg.norm(self.max_height - self.goal)) + height_reward = -10*np.linalg.norm(self.max_height - self.goal) + # no healthy reward when using context, because we optimize a negative value + healthy_reward = 0 rewards = height_reward + healthy_reward @@ -105,7 +103,6 @@ if __name__ == '__main__': obs = env.reset() for i in range(2000): - # objective.load_result("/tmp/cma") # test with random actions ac = env.action_space.sample() obs, rew, d, info = env.step(ac) @@ -114,4 +111,4 @@ if __name__ == '__main__': if d: env.reset() - env.close() \ No newline at end of file + env.close() diff --git a/alr_envs/alr/mujoco/beerpong/beerpong.py b/alr_envs/alr/mujoco/beerpong/beerpong.py index 11c09e4..218df6d 100644 --- a/alr_envs/alr/mujoco/beerpong/beerpong.py +++ b/alr_envs/alr/mujoco/beerpong/beerpong.py @@ -7,16 +7,6 @@ from gym.envs.mujoco import MujocoEnv from alr_envs.alr.mujoco.beerpong.beerpong_reward_staged import BeerPongReward -CUP_POS_MIN = np.array([-1.42, -4.05]) -CUP_POS_MAX = np.array([1.42, -1.25]) - - -# CUP_POS_MIN = np.array([-0.32, -2.2]) -# CUP_POS_MAX = np.array([0.32, -1.2]) - -# smaller context space -> Easier task -# CUP_POS_MIN = np.array([-0.16, -2.2]) -# CUP_POS_MAX = np.array([0.16, -1.7]) class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): def __init__(self, frame_skip=2, apply_gravity_comp=True): @@ -27,10 +17,16 @@ class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): self.cup_goal_pos = np.array(cup_goal_pos) self._steps = 0 + # Small Context -> Easier. Todo: Should we do different versions? # self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets", # "beerpong_wo_cup" + ".xml") + # self.cup_pos_min = np.array([-0.32, -2.2]) + # self.cup_pos_max = np.array([0.32, -1.2]) + self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets", "beerpong_wo_cup_big_table" + ".xml") + self.cup_pos_min = np.array([-1.42, -4.05]) + self.cup_pos_max = np.array([1.42, -1.25]) self.j_min = np.array([-2.6, -1.985, -2.8, -0.9, -4.55, -1.5707, -2.7]) self.j_max = np.array([2.6, 1.985, 2.8, 3.14159, 1.25, 1.5707, 2.7]) @@ -49,9 +45,7 @@ class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): self.cup_table_id = 10 self.add_noise = False - - reward_function = BeerPongReward - self.reward_function = reward_function() + self.reward_function = BeerPongReward() self.repeat_action = frame_skip MujocoEnv.__init__(self, self.xml_path, frame_skip=1) utils.EzPickle.__init__(self) @@ -78,10 +72,11 @@ class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): start_pos = init_pos_all start_pos[0:7] = init_pos_robot + # TODO: Ask Max why we need to set the state twice. self.set_state(start_pos, init_vel) start_pos[7::] = self.sim.data.site_xpos[self.ball_site_id, :].copy() self.set_state(start_pos, init_vel) - xy = self.np_random.uniform(CUP_POS_MIN, CUP_POS_MAX) + xy = self.np_random.uniform(self.cup_pos_min, self.cup_pos_max) xyz = np.zeros(3) xyz[:2] = xy xyz[-1] = 0.840 @@ -89,9 +84,7 @@ class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): return self._get_obs() def step(self, a): - reward_dist = 0.0 - angular_vel = 0.0 - + crash = False for _ in range(self.repeat_action): if self.apply_gravity_comp: applied_action = a + self.sim.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0] @@ -100,7 +93,7 @@ class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): try: self.do_simulation(applied_action, self.frame_skip) self.reward_function.initialize(self) - self.reward_function.check_contacts(self.sim) + # self.reward_function.check_contacts(self.sim) # I assume this is not important? if self._steps < self.release_step: self.sim.data.qpos[7::] = self.sim.data.site_xpos[self.ball_site_id, :].copy() self.sim.data.qvel[7::] = self.sim.data.site_xvelp[self.ball_site_id, :].copy() @@ -112,34 +105,19 @@ class ALRBeerBongEnv(MujocoEnv, utils.EzPickle): if not crash: reward, reward_infos = self.reward_function.compute_reward(self, applied_action) - success = reward_infos['success'] is_collided = reward_infos['is_collided'] - ball_pos = reward_infos['ball_pos'] - ball_vel = reward_infos['ball_vel'] done = is_collided or self._steps == self.ep_length - 1 self._steps += 1 else: reward = -30 - reward_infos = dict() - success = False - is_collided = False done = True - ball_pos = np.zeros(3) - ball_vel = np.zeros(3) + reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False} infos = dict( - reward_dist=reward_dist, reward=reward, - velocity=angular_vel, - # traj=self._q_pos, action=a, q_pos=self.sim.data.qpos[0:7].ravel().copy(), - q_vel=self.sim.data.qvel[0:7].ravel().copy(), - ball_pos=ball_pos, - ball_vel=ball_vel, - success=success, - is_collided=is_collided, sim_crash=crash, - table_contact_first=int(not self.reward_function.ball_ground_contact_first) + q_vel=self.sim.data.qvel[0:7].ravel().copy(), sim_crash=crash, ) infos.update(reward_infos) return ob, reward, done, infos @@ -239,14 +217,7 @@ if __name__ == "__main__": env.reset() env.render("human") for i in range(1500): - # ac = 10 * env.action_space.sample() - ac = np.ones(7) - # ac = np.zeros(7) - # ac[0] = 0 - # ac[1] = -0.01 - # ac[3] = -0.01 - # if env._steps > 150: - # ac[0] = 1 + ac = 10 * env.action_space.sample() obs, rew, d, info = env.step(ac) env.render("human") print(env.dt) diff --git a/alr_envs/alr/mujoco/beerpong/beerpong_reward_staged.py b/alr_envs/alr/mujoco/beerpong/beerpong_reward_staged.py index cd2ab75..807fb19 100644 --- a/alr_envs/alr/mujoco/beerpong/beerpong_reward_staged.py +++ b/alr_envs/alr/mujoco/beerpong/beerpong_reward_staged.py @@ -21,12 +21,9 @@ class BeerPongReward: self.cup_collision_objects = ["cup_geom_table3", "cup_geom_table4", "cup_geom_table5", "cup_geom_table6", "cup_geom_table7", "cup_geom_table8", "cup_geom_table9", "cup_geom_table10", - # "cup_base_table", "cup_base_table_contact", "cup_geom_table15", "cup_geom_table16", "cup_geom_table17", "cup_geom1_table8", - # "cup_base_table_contact", - # "cup_base_table" ] self.dists = None @@ -39,7 +36,7 @@ class BeerPongReward: self.ball_in_cup = False self.dist_ground_cup = -1 # distance floor to cup if first floor contact - ### IDs + # IDs self.ball_collision_id = None self.table_collision_id = None self.wall_collision_id = None @@ -96,10 +93,10 @@ class BeerPongReward: self.action_costs.append(np.copy(action_cost)) # # ##################### Reward function which does not force to bounce once on the table (quad dist) ######### - # Comment Onur: Is this needed? + # Is this needed? # self._is_collided = self._check_collision_with_itself(env.sim, self.robot_collision_ids) - if env._steps == env.ep_length - 1:# or self._is_collided: + if env._steps == env.ep_length - 1: # or self._is_collided: min_dist = np.min(self.dists) final_dist = self.dists_final[-1] if self.ball_ground_contact_first: @@ -128,9 +125,10 @@ class BeerPongReward: reward = - action_cost success = False # ############################################################################################################## - infos = {"success": success, "ball_pos": ball_pos.copy(), - "ball_vel": ball_vel.copy(), "action_cost": action_cost, "task_reward": reward, "is_collided": False} # TODO: Check if is collided is needed - + infos = {"success": success, "ball_pos": ball_pos.copy(), + "ball_vel": ball_vel.copy(), "action_cost": action_cost, "task_reward": reward, + "table_contact_first": int(not self.ball_ground_contact_first), + "is_collided": False} # TODO: Check if is collided is needed return reward, infos def check_contacts(self, sim): diff --git a/alr_envs/alr/mujoco/walker_2d_jump/assets/walker2d.xml b/alr_envs/alr/mujoco/walker_2d_jump/assets/walker2d.xml index 1b48e36..f3bcbd1 100644 --- a/alr_envs/alr/mujoco/walker_2d_jump/assets/walker2d.xml +++ b/alr_envs/alr/mujoco/walker_2d_jump/assets/walker2d.xml @@ -21,6 +21,7 @@ + @@ -34,6 +35,7 @@ + @@ -59,4 +61,4 @@ - \ No newline at end of file + diff --git a/alr_envs/alr/mujoco/walker_2d_jump/walker_2d_jump.py b/alr_envs/alr/mujoco/walker_2d_jump/walker_2d_jump.py index 009dd9d..1ab0d29 100644 --- a/alr_envs/alr/mujoco/walker_2d_jump/walker_2d_jump.py +++ b/alr_envs/alr/mujoco/walker_2d_jump/walker_2d_jump.py @@ -4,6 +4,10 @@ import numpy as np MAX_EPISODE_STEPS_WALKERJUMP = 300 +# TODO: Right now this environment only considers jumping to a specific height, which is not nice. It should be extended +# to the same structure as the Hopper, where the angles are randomized (->contexts) and the agent should jump as height +# as possible, while landing at a specific target position + class ALRWalker2dJumpEnv(Walker2dEnv): """ @@ -21,14 +25,12 @@ class ALRWalker2dJumpEnv(Walker2dEnv): healthy_angle_range=(-1.0, 1.0), reset_noise_scale=5e-3, penalty=0, - context=True, exclude_current_positions_from_observation=True, max_episode_steps=300): self.current_step = 0 self.max_episode_steps = max_episode_steps self.max_height = 0 self._penalty = penalty - self.context = context self.goal = 0 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, @@ -43,31 +45,24 @@ class ALRWalker2dJumpEnv(Walker2dEnv): self.max_height = max(height, self.max_height) - fell_over = height < 0.2 - done = fell_over + done = height < 0.2 ctrl_cost = self.control_cost(action) costs = ctrl_cost - + rewards = 0 if self.current_step >= self.max_episode_steps or done: done = True - height_goal_distance = -10 * (np.linalg.norm(self.max_height - self.goal)) if self.context else self.max_height + height_goal_distance = -10 * (np.linalg.norm(self.max_height - self.goal)) healthy_reward = self.healthy_reward * self.current_step rewards = height_goal_distance + healthy_reward - else: - # penalty not needed - rewards = 0 - rewards += ((action[:2] > 0) * self._penalty).sum() if self.current_step < 4 else 0 - rewards += ((action[3:5] > 0) * self._penalty).sum() if self.current_step < 4 else 0 - observation = self._get_obs() reward = rewards - costs info = { 'height': height, 'max_height': self.max_height, - 'goal' : self.goal, + 'goal': self.goal, } return observation, reward, done, info @@ -78,7 +73,7 @@ class ALRWalker2dJumpEnv(Walker2dEnv): def reset(self): self.current_step = 0 self.max_height = 0 - self.goal = np.random.uniform(1.5, 2.5, 1) # 1.5 3.0 + self.goal = np.random.uniform(1.5, 2.5, 1) # 1.5 3.0 return super().reset() # overwrite reset_model to make it deterministic @@ -99,8 +94,7 @@ if __name__ == '__main__': env = ALRWalker2dJumpEnv() obs = env.reset() - for i in range(2000): - # objective.load_result("/tmp/cma") + for i in range(6000): # test with random actions ac = env.action_space.sample() obs, rew, d, info = env.step(ac) @@ -110,4 +104,4 @@ if __name__ == '__main__': print('After ', i, ' steps, done: ', d) env.reset() - env.close() \ No newline at end of file + env.close() diff --git a/alr_envs/utils/make_env_helpers.py b/alr_envs/utils/make_env_helpers.py index b5587a7..57b4c69 100644 --- a/alr_envs/utils/make_env_helpers.py +++ b/alr_envs/utils/make_env_helpers.py @@ -7,12 +7,12 @@ import numpy as np from gym.envs.registration import EnvSpec, registry from gym.wrappers import TimeAwareObservation -from alr_envs.mp.basis_generator_factory import get_basis_generator -from alr_envs.mp.black_box_wrapper import BlackBoxWrapper -from alr_envs.mp.controllers.controller_factory import get_controller -from alr_envs.mp.mp_factory import get_trajectory_generator -from alr_envs.mp.phase_generator_factory import get_phase_generator -from alr_envs.mp.raw_interface_wrapper import RawInterfaceWrapper +from alr_envs.black_box.black_box_wrapper import BlackBoxWrapper +from alr_envs.black_box.controller.controller_factory import get_controller +from alr_envs.black_box.factory.basis_generator_factory import get_basis_generator +from alr_envs.black_box.factory.phase_generator_factory import get_phase_generator +from alr_envs.black_box.factory.trajectory_generator_factory import get_trajectory_generator +from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper from alr_envs.utils.utils import nested_update