updates

2021-02-24 15:37:54 +01:00
parent 60e1673ee1
commit 7e988758fe
52 changed files with 550 additions and 13 deletions
@@ -1,5 +1,5 @@
 <mujoco model="wam(v1.31)">
-    <compiler angle="radian" meshdir="meshes/" />
+    <compiler angle="radian" meshdir="../../meshes/wam/" />
    <option timestep="0.0005" integrator="Euler" />
    <size njmax="500" nconmax="100" />
    <default class="main">
@@ -286,7 +286,7 @@
                                                                                                                                                                    <joint name="J1_29" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
                                                                                                                                                                    <geom name="G29" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
                                                                                                                                                                    <body name="ball">
-                                                                                                                                                                        <geom name="ball_geom" type="sphere" size="0.02" mass="0.021" rgba="0.8 0.2 0.1 1"/>
+                                                                                                                                                                        <geom name="ball_geom" type="sphere" size="0.02" mass="0.015" rgba="0.8 0.2 0.1 1"/>
                                                                                                                                                                    </body>
                                                                                                                                                                </body>
                                                                                                                                                            </body>
@@ -2,8 +2,6 @@ from gym import utils
 import os
 import numpy as np
 from alr_envs.mujoco import alr_mujoco_env
-from alr_envs.mujoco.ball_in_a_cup.ball_in_a_cup_reward import BallInACupReward
-import mujoco_py


 class ALRBallInACupEnv(alr_mujoco_env.AlrMujocoEnv, utils.EzPickle):
@@ -116,14 +114,14 @@ if __name__ == "__main__":

    env.configure(ctxt)
    env.reset()
-    env.render()
-    for i in range(2000):
+    # env.render()
+    for i in range(16000):
        # test with random actions
-        ac = 0.01 * env.action_space.sample()[0:7]
+        ac = 0.001 * env.action_space.sample()[0:7]
        # ac = env.start_pos
        # ac[0] += np.pi/2
        obs, rew, d, info = env.step(ac)
-        env.render()
+        # env.render()

        print(rew)

@@ -31,13 +31,15 @@ class BallInACupReward(alr_reward_fct.AlrReward):

    def reset(self, context):
        self.ball_traj = np.zeros(shape=(self.sim_time, 3))
+        self.cup_traj = np.zeros(shape=(self.sim_time, 3))
        self.dists = []
        self.dists_ctxt = []
        self.dists_final = []
        self.costs = []
        self.context = context
        self.ball_in_cup = False
-        self.dist_ctxt = 5
+        self.ball_above_threshold = False
+        self.dist_ctxt = 3

    def compute_reward(self, action, sim, step):
        action_cost = np.sum(np.square(action))
@@ -64,6 +66,7 @@ class BallInACupReward(alr_reward_fct.AlrReward):
        self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
        self.dists_ctxt.append(np.linalg.norm(ball_pos - self.context))
        self.ball_traj[step, :] = ball_pos
+        self.cup_traj[step, :] = goal_pos

        # Determine the first time when ball is in cup
        if not self.ball_in_cup:
@@ -78,9 +81,18 @@ class BallInACupReward(alr_reward_fct.AlrReward):
            dist_final = self.dists_final[-1]
            # dist_ctxt = self.dists_ctxt[-1]

+             # max distance between ball and cup and cup height at that time
+            ball_to_cup_diff = self.ball_traj[:, 2] - self.cup_traj[:, 2]
+            t_max_diff = np.argmax(ball_to_cup_diff)
+            t_max_ball_height = np.argmax(self.ball_traj[:, 2])
+            max_ball_height = np.max(self.ball_traj[:, 2])
+
            # cost = self._get_stage_wise_cost(ball_in_cup, min_dist, dist_final, dist_ctxt)
-            cost = 2 * (0.5 * min_dist + 0.5 * dist_final + 0.1 * self.dist_ctxt)
+            cost = 0.3 * min_dist + 0.3 * dist_final + 0.3 * np.minimum(self.dist_ctxt, 3)
            reward = np.exp(-1 * cost) - 1e-4 * action_cost
+            if max_ball_height < self.context[2] or ball_to_cup_diff[t_max_ball_height] < 0:
+                reward -= 1
+
            success = dist_final < 0.05 and self.dist_ctxt < 0.05
        else:
            reward = - 1e-4 * action_cost
@@ -3,7 +3,7 @@ from alr_envs.mujoco.ball_in_a_cup.ball_in_a_cup import ALRBallInACupEnv
 from alr_envs.mujoco.ball_in_a_cup.ball_in_a_cup_simple import ALRBallInACupEnv as ALRBallInACupEnvSimple


-def make_env(rank, seed=0):
+def make_contextual_env(rank, seed=0):
    """
    Utility function for multiprocessed env.

@@ -26,7 +26,41 @@ def make_env(rank, seed=0):
                               duration=3.5,
                               post_traj_time=4.5,
                               dt=env.dt,
-                               weights_scale=0.1,
+                               weights_scale=0.5,
+                               zero_start=True,
+                               zero_goal=True
+                               )
+
+        env.seed(seed + rank)
+        return env
+
+    return _init
+
+
+def make_env(rank, seed=0):
+    """
+    Utility function for multiprocessed env.
+
+    :param env_id: (str) the environment ID
+    :param num_env: (int) the number of environments you wish to have in subprocesses
+    :param seed: (int) the initial seed for RNG
+    :param rank: (int) index of the subprocess
+    :returns a function that generates an environment
+    """
+
+    def _init():
+        env = ALRBallInACupEnvSimple()
+
+        env = DetPMPEnvWrapper(env,
+                               num_dof=7,
+                               num_basis=5,
+                               width=0.005,
+                               policy_type="motor",
+                               start_pos=env.start_pos,
+                               duration=3.5,
+                               post_traj_time=4.5,
+                               dt=env.dt,
+                               weights_scale=0.5,
                               zero_start=True,
                               zero_goal=True
                               )
@@ -60,7 +94,7 @@ def make_simple_env(rank, seed=0):
                               duration=3.5,
                               post_traj_time=4.5,
                               dt=env.dt,
-                               weights_scale=0.1,
+                               weights_scale=0.5,
                               zero_start=True,
                               zero_goal=True
                               )