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

finish up beerpong, walker2d and ant needs more extensions, fix import bugs.

Browse Source
This commit is contained in:
Onur 2022-07-01 09:54:42 +02:00
parent 4437ab9577
commit d4e3b957a9
9 changed files with 62 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
alr_envs/alr/__init__.py
View File

@ -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
}
)

2
alr_envs/alr/classic_control/hole_reacher/mp_wrapper.py
View File

@ -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):

1
alr_envs/alr/mujoco/__init__.py
View File

@ -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

13
alr_envs/alr/mujoco/ant_jump/ant_jump.py
View File

@ -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
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)

57
alr_envs/alr/mujoco/beerpong/beerpong.py
View File

@ -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)

12
alr_envs/alr/mujoco/beerpong/beerpong_reward_staged.py
View File

@ -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,7 +93,7 @@ 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:
@ -129,8 +126,9 @@ class BeerPongReward:
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
"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):

2
alr_envs/alr/mujoco/walker_2d_jump/assets/walker2d.xml
View File

@ -21,6 +21,7 @@
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.2/2 0 0.1">
<site name="foot_right_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="0 0 1 1" type="sphere"/>
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="0.9" fromto="-0.0 0 0.1 0.2 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
@ -34,6 +35,7 @@
<joint axis="0 -1 0" name="leg_left_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_left_geom" rgba=".7 .3 .6 1" size="0.04" type="capsule"/>
<body name="foot_left" pos="0.2/2 0 0.1">
<site name="foot_left_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
<joint axis="0 -1 0" name="foot_left_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="1.9" fromto="-0.0 0 0.1 0.2 0 0.1" name="foot_left_geom" rgba=".7 .3 .6 1" size="0.06" type="capsule"/>
</body>

22
alr_envs/alr/mujoco/walker_2d_jump/walker_2d_jump.py
View File

@ -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,24 +45,17 @@ 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
@ -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)

12
alr_envs/utils/make_env_helpers.py
View File

@ -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