Added environments from Paul + Marc

2022-04-13 17:28:25 +02:00
parent 7ffe94dcfd
commit 092525889b
26 changed files with 1795 additions and 2 deletions
@@ -0,0 +1 @@
+from .mp_wrapper import MPWrapper
@@ -0,0 +1,48 @@
+<mujoco model="hopper">
+  <compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
+  <default>
+    <joint armature="1" damping="1" limited="true"/>
+    <geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
+    <motor ctrllimited="true" ctrlrange="-.4 .4"/>
+  </default>
+  <option integrator="RK4" timestep="0.002"/>
+  <visual>
+    <map znear="0.02"/>
+  </visual>
+  <worldbody>
+    <light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
+    <geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
+    <body name="torso" pos="0 0 1.25">
+      <camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
+      <joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
+      <joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
+      <joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
+      <geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
+      <body name="thigh" pos="0 0 1.05">
+        <joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
+        <geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
+        <body name="leg" pos="0 0 0.35">
+          <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.13/2 0 0.1">
+            <joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
+            <geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <actuator>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
+  </actuator>
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
+            width="100" height="100"/>
+        <texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
+        <texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
+        <material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
+        <material name="geom" texture="texgeom" texuniform="true"/>
+    </asset>
+</mujoco>
@@ -0,0 +1,51 @@
+<mujoco model="hopper">
+  <compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
+  <default>
+    <joint armature="1" damping="1" limited="true"/>
+    <geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
+    <motor ctrllimited="true" ctrlrange="-.4 .4"/>
+  </default>
+  <option integrator="RK4" timestep="0.002"/>
+  <visual>
+    <map znear="0.02"/>
+  </visual>
+  <worldbody>
+    <light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
+    <geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
+    <body name="torso" pos="0 0 1.25">
+      <camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
+      <joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
+      <joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
+      <joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
+      <geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
+      <body name="thigh" pos="0 0 1.05">
+        <joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
+        <geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
+        <body name="leg" pos="0 0 0.35">
+          <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.13/2 0 0.1">
+            <joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
+            <geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
+          </body>
+        </body>
+      </body>
+    </body>
+    <body name="box" pos="1 0 0">
+        <geom friction="1.0" fromto="0.48 0 0 1 0 0" name="basket_ground_geom" size="0.3" type="box" rgba="1 0 0 1"/>
+    </body>
+  </worldbody>
+  <actuator>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
+  </actuator>
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
+            width="100" height="100"/>
+        <texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
+        <texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
+        <material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
+        <material name="geom" texture="texgeom" texuniform="true"/>
+    </asset>
+</mujoco>
@@ -0,0 +1,110 @@
+from gym.envs.mujoco.hopper_v3 import HopperEnv
+import numpy as np
+
+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'))
+    - healthy_z_range: (0.7, float('inf')) -> (0.5, float('inf'))
+
+    """
+
+    def __init__(self,
+                 xml_file='hopper.xml',
+                 forward_reward_weight=1.0,
+                 ctrl_cost_weight=1e-3,
+                 healthy_reward=0.0,
+                 penalty=0.0,
+                 context=True,
+                 terminate_when_unhealthy=True,
+                 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=True,
+                 max_episode_steps=250):
+        self.current_step = 0
+        self.max_height = 0
+        self.max_episode_steps = max_episode_steps
+        self.penalty = penalty
+        self.goal = 0
+        self.context = context
+        self.exclude_current_positions_from_observation = exclude_current_positions_from_observation
+        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.current_step += 1
+        self.do_simulation(action, self.frame_skip)
+        height_after = self.get_body_com("torso")[2]
+        self.max_height = max(height_after, self.max_height)
+
+        ctrl_cost = self.control_cost(action)
+        costs = ctrl_cost
+        done = False
+
+        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 * 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
+
+        else:
+            # penalty for wrong start direction of first two joints; not needed, could be removed
+            rewards = ((action[:2] > 0) * self.penalty).sum() if self.current_step < 10 else 0
+
+        observation = self._get_obs()
+        reward = rewards - costs
+        info = {
+            'height'    : height_after,
+            'max_height': self.max_height,
+            'goal' : self.goal
+        }
+
+        return observation, reward, done, info
+
+    def _get_obs(self):
+        return np.append(super()._get_obs(), self.goal)
+
+    def reset(self):
+        self.goal = np.random.uniform(1.4, 2.3, 1) # 1.3 2.3
+        self.max_height = 0
+        self.current_step = 0
+        return super().reset()
+
+    # overwrite reset_model to make it deterministic
+    def reset_model(self):
+        noise_low = -self._reset_noise_scale
+        noise_high = self._reset_noise_scale
+
+        qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
+        qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
+
+        self.set_state(qpos, qvel)
+
+        observation = self._get_obs()
+        return observation
+
+if __name__ == '__main__':
+    render_mode = "human"  # "human" or "partial" or "final"
+    env = ALRHopperJumpEnv()
+    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)
+        if i % 10 == 0:
+            env.render(mode=render_mode)
+        if d:
+            print('After ', i, ' steps, done: ', d)
+            env.reset()
+
+    env.close()
@@ -0,0 +1,170 @@
+from gym.envs.mujoco.hopper_v3 import HopperEnv
+
+import numpy as np
+import os
+
+MAX_EPISODE_STEPS_HOPPERJUMPONBOX = 250
+
+
+class ALRHopperJumpOnBoxEnv(HopperEnv):
+    """
+    Initialization changes to normal Hopper:
+    - healthy_reward: 1.0 -> 0.01 -> 0.001
+    - healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
+    """
+
+    def __init__(self,
+                 xml_file='hopper_jump_on_box.xml',
+                 forward_reward_weight=1.0,
+                 ctrl_cost_weight=1e-3,
+                 healthy_reward=0.001,
+                 terminate_when_unhealthy=True,
+                 healthy_state_range=(-100.0, 100.0),
+                 healthy_z_range=(0.7, float('inf')),
+                 healthy_angle_range=(-float('inf'), float('inf')),
+                 reset_noise_scale=5e-3,
+                 context=True,
+                 exclude_current_positions_from_observation=True,
+                 max_episode_steps=250):
+        self.current_step = 0
+        self.max_height = 0
+        self.max_episode_steps = max_episode_steps
+        self.min_distance = 5000  # what value?
+        self.hopper_on_box = False
+        self.context = context
+        self.box_x = 1
+        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.current_step += 1
+        self.do_simulation(action, self.frame_skip)
+        height_after = self.get_body_com("torso")[2]
+        foot_pos = self.get_body_com("foot")
+        self.max_height = max(height_after, self.max_height)
+
+        vx, vz, vangle = self.sim.data.qvel[0:3]
+
+        s = self.state_vector()
+        fell_over = not (np.isfinite(s).all() and (np.abs(s[2:]) < 100).all() and (height_after > .7))
+
+        box_pos = self.get_body_com("box")
+        box_size = 0.3
+        box_height = 0.3
+        box_center = (box_pos[0] + (box_size / 2), box_pos[1], box_height)
+        foot_length = 0.3
+        foot_center = foot_pos[0] - (foot_length / 2)
+
+        dist = np.linalg.norm(foot_pos - box_center)
+
+        self.min_distance = min(dist, self.min_distance)
+
+        # check if foot is on box
+        is_on_box_x = box_pos[0] <= foot_center <= box_pos[0] + box_size
+        is_on_box_y = True  # is y always true because he can only move in x and z direction?
+        is_on_box_z = box_height - 0.02 <= foot_pos[2] <= box_height + 0.02
+        is_on_box = is_on_box_x and is_on_box_y and is_on_box_z
+        if is_on_box: self.hopper_on_box = True
+
+        ctrl_cost = self.control_cost(action)
+
+        costs = ctrl_cost
+
+        done = fell_over or self.hopper_on_box
+
+        if self.current_step >= self.max_episode_steps or done:
+            done = False
+
+            max_height = self.max_height.copy()
+            min_distance = self.min_distance.copy()
+
+            alive_bonus = self._healthy_reward * self.current_step
+            box_bonus = 0
+            rewards = 0
+
+            # TODO explain what we did here for the calculation of the reward
+            if is_on_box:
+                if self.context:
+                    rewards -= 100 * vx ** 2 if 100 * vx ** 2 < 1 else 1
+                else:
+                    box_bonus = 10
+                    rewards += box_bonus
+                    # rewards -= dist * dist ???? why when already on box?
+                    # reward -= 90 - abs(angle)
+                    rewards -= 100 * vx ** 2 if 100 * vx ** 2 < 1 else 1
+                    rewards += max_height * 3
+                    rewards += alive_bonus
+
+            else:
+                if self.context:
+                    rewards = -10 - min_distance
+                    rewards += max_height * 3
+                else:
+                    # reward -= (dist*dist)
+                    rewards -= min_distance * min_distance
+                    # rewards -= dist / self.max_distance
+                    rewards += max_height
+                    rewards += alive_bonus
+
+        else:
+            rewards = 0
+
+        observation = self._get_obs()
+        reward = rewards - costs
+        info = {
+            'height': height_after,
+            'max_height': self.max_height.copy(),
+            'min_distance': self.min_distance,
+            'goal': self.box_x,
+        }
+
+        return observation, reward, done, info
+
+    def _get_obs(self):
+        return np.append(super()._get_obs(), self.box_x)
+
+    def reset(self):
+
+        self.max_height = 0
+        self.min_distance = 5000
+        self.current_step = 0
+        self.hopper_on_box = False
+        if self.context:
+            box_id = self.sim.model.body_name2id("box")
+            self.box_x = np.random.uniform(1, 3, 1)
+            self.sim.model.body_pos[box_id] = [self.box_x, 0, 0]
+        return super().reset()
+
+    # overwrite reset_model to make it deterministic
+    def reset_model(self):
+        noise_low = -self._reset_noise_scale
+        noise_high = self._reset_noise_scale
+
+        qpos = self.init_qpos  # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
+        qvel = self.init_qvel  # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
+
+        self.set_state(qpos, qvel)
+
+        observation = self._get_obs()
+        return observation
+
+if __name__ == '__main__':
+    render_mode = "human"  # "human" or "partial" or "final"
+    env = ALRHopperJumpOnBoxEnv()
+    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)
+        if i % 10 == 0:
+            env.render(mode=render_mode)
+        if d:
+            print('After ', i, ' steps, done: ', d)
+            env.reset()
+
+    env.close()
@@ -0,0 +1,31 @@
+from typing import Tuple, Union
+
+import numpy as np
+
+from mp_env_api import MPEnvWrapper
+
+
+class MPWrapper(MPEnvWrapper):
+    @property
+    def active_obs(self):
+        return np.hstack([
+            [False] * (5 + int(not self.exclude_current_positions_from_observation)),  # position
+            [False] * 6,  # velocity
+            [True]
+        ])
+
+    @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