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,117 @@
+import numpy as np
+from gym.envs.mujoco.ant_v3 import AntEnv
+
+MAX_EPISODE_STEPS_ANTJUMP = 200
+
+
+class ALRAntJumpEnv(AntEnv):
+    """
+    Initialization changes to normal Ant:
+    - healthy_reward: 1.0 -> 0.01 -> 0.0 no healthy reward needed - Paul and Marc
+    - ctrl_cost_weight 0.5 -> 0.0
+    - contact_cost_weight: 5e-4 -> 0.0
+    - healthy_z_range: (0.2, 1.0) -> (0.3, float('inf'))  !!!!! Does that make sense, limiting height?
+    """
+
+    def __init__(self,
+                 xml_file='ant.xml',
+                 ctrl_cost_weight=0.0,
+                 contact_cost_weight=0.0,
+                 healthy_reward=0.0,
+                 terminate_when_unhealthy=True,
+                 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
+        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)
+
+    def step(self, action):
+
+        self.current_step += 1
+        self.do_simulation(action, self.frame_skip)
+
+        height = self.get_body_com("torso")[2].copy()
+
+        self.max_height = max(height, self.max_height)
+
+        rewards = 0
+
+        ctrl_cost = self.control_cost(action)
+        contact_cost = self.contact_cost
+
+        costs = ctrl_cost + contact_cost
+
+        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
+
+        obs = self._get_obs()
+        reward = rewards - costs
+
+        info = {
+            'height': height,
+            'max_height': self.max_height,
+            'goal': self.goal
+        }
+
+        return obs, reward, done, info
+
+    def _get_obs(self):
+        return np.append(super()._get_obs(), self.goal)
+
+    def reset(self):
+        self.current_step = 0
+        self.max_height = 0
+        self.goal = np.random.uniform(1.0, 2.5,
+                                      1)  # goal heights from 1.0 to 2.5; can be increased, but didnt work well with CMORE
+        return super().reset()
+
+    # reset_model had to be implemented in every env 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 = ALRAntJumpEnv()
+    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:
+            env.reset()
+
+    env.close()
@@ -0,0 +1,81 @@
+<mujoco model="ant">
+  <compiler angle="degree" coordinate="local" inertiafromgeom="true"/>
+  <option integrator="RK4" timestep="0.01"/>
+  <custom>
+    <numeric data="0.0 0.0 0.55 1.0 0.0 0.0 0.0 0.0 1.0 0.0 -1.0 0.0 -1.0 0.0 1.0" name="init_qpos"/>
+  </custom>
+  <default>
+    <joint armature="1" damping="1" limited="true"/>
+    <geom conaffinity="0" condim="3" density="5.0" friction="1 0.5 0.5" margin="0.01" rgba="0.8 0.6 0.4 1"/>
+  </default>
+  <asset>
+    <texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="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>
+  <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" material="MatPlane" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="40 40 40" type="plane"/>
+    <body name="torso" pos="0 0 0.75">
+      <camera name="track" mode="trackcom" pos="0 -3 0.3" xyaxes="1 0 0 0 0 1"/>
+      <geom name="torso_geom" pos="0 0 0" size="0.25" type="sphere"/>
+      <joint armature="0" damping="0" limited="false" margin="0.01" name="root" pos="0 0 0" type="free"/>
+      <body name="front_left_leg" pos="0 0 0">
+        <geom fromto="0.0 0.0 0.0 0.2 0.2 0.0" name="aux_1_geom" size="0.08" type="capsule"/>
+        <body name="aux_1" pos="0.2 0.2 0">
+          <joint axis="0 0 1" name="hip_1" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
+          <geom fromto="0.0 0.0 0.0 0.2 0.2 0.0" name="left_leg_geom" size="0.08" type="capsule"/>
+          <body pos="0.2 0.2 0">
+            <joint axis="-1 1 0" name="ankle_1" pos="0.0 0.0 0.0" range="30 70" type="hinge"/>
+            <geom fromto="0.0 0.0 0.0 0.4 0.4 0.0" name="left_ankle_geom" size="0.08" type="capsule"/>
+          </body>
+        </body>
+      </body>
+      <body name="front_right_leg" pos="0 0 0">
+        <geom fromto="0.0 0.0 0.0 -0.2 0.2 0.0" name="aux_2_geom" size="0.08" type="capsule"/>
+        <body name="aux_2" pos="-0.2 0.2 0">
+          <joint axis="0 0 1" name="hip_2" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
+          <geom fromto="0.0 0.0 0.0 -0.2 0.2 0.0" name="right_leg_geom" size="0.08" type="capsule"/>
+          <body pos="-0.2 0.2 0">
+            <joint axis="1 1 0" name="ankle_2" pos="0.0 0.0 0.0" range="-70 -30" type="hinge"/>
+            <geom fromto="0.0 0.0 0.0 -0.4 0.4 0.0" name="right_ankle_geom" size="0.08" type="capsule"/>
+          </body>
+        </body>
+      </body>
+      <body name="back_leg" pos="0 0 0">
+        <geom fromto="0.0 0.0 0.0 -0.2 -0.2 0.0" name="aux_3_geom" size="0.08" type="capsule"/>
+        <body name="aux_3" pos="-0.2 -0.2 0">
+          <joint axis="0 0 1" name="hip_3" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
+          <geom fromto="0.0 0.0 0.0 -0.2 -0.2 0.0" name="back_leg_geom" size="0.08" type="capsule"/>
+          <body pos="-0.2 -0.2 0">
+            <joint axis="-1 1 0" name="ankle_3" pos="0.0 0.0 0.0" range="-70 -30" type="hinge"/>
+            <geom fromto="0.0 0.0 0.0 -0.4 -0.4 0.0" name="third_ankle_geom" size="0.08" type="capsule"/>
+          </body>
+        </body>
+      </body>
+      <body name="right_back_leg" pos="0 0 0">
+        <geom fromto="0.0 0.0 0.0 0.2 -0.2 0.0" name="aux_4_geom" size="0.08" type="capsule"/>
+        <body name="aux_4" pos="0.2 -0.2 0">
+          <joint axis="0 0 1" name="hip_4" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
+          <geom fromto="0.0 0.0 0.0 0.2 -0.2 0.0" name="rightback_leg_geom" size="0.08" type="capsule"/>
+          <body pos="0.2 -0.2 0">
+            <joint axis="1 1 0" name="ankle_4" pos="0.0 0.0 0.0" range="30 70" type="hinge"/>
+            <geom fromto="0.0 0.0 0.0 0.4 -0.4 0.0" name="fourth_ankle_geom" size="0.08" type="capsule"/>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <actuator>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_4" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_4" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_1" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_1" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_2" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_2" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_3" gear="150"/>
+    <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_3" gear="150"/>
+  </actuator>
+</mujoco>
@@ -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] * 111, # ant has 111 dimensional observation space !!
+            [True] # goal height
+        ])
+
+    @property
+    def current_pos(self) -> Union[float, int, np.ndarray]:
+        return self.env.sim.data.qpos[7:15].copy()
+
+    @property
+    def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
+        return self.env.sim.data.qvel[6:14].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