updates

alr_envs/classic_control/hole_reacher.py

@@ -83,7 +83,7 @@ class HoleReacher(gym.Env):
         """
         a single step with an action in joint velocity space
         """
-        vel = action
+        vel = action  # + 0.01 * np.random.randn(self.num_links)
         acc = (vel - self._angle_velocity) / self.dt
         self._angle_velocity = vel
         self._joint_angles = self._joint_angles + self.dt * self._angle_velocity
@@ -96,20 +96,20 @@ class HoleReacher(gym.Env):
 
         dist_reward = 0
         if not self._is_collided:
-            if self._steps == 180:
+            if self._steps == 199:
                 dist_reward = np.linalg.norm(self.end_effector - self.bottom_center_of_hole)
         else:
             dist_reward = np.linalg.norm(self.end_effector - self.bottom_center_of_hole)
 
         reward = - dist_reward ** 2
 
-        reward -= 1e-6 * np.sum(acc**2)
+        reward -= 5e-8 * np.sum(acc**2)
 
-        if self._steps == 180:
-            reward -= 0.1 * np.sum(vel**2) ** 2
+        # if self._steps == 180:
+        #     reward -= 0.1 * np.sum(vel**2) ** 2
 
         if self._is_collided:
-            reward -= self.collision_penalty
+            reward = -self.collision_penalty
 
         info = {"is_collided": self._is_collided}
 

alr_envs/classic_control/utils.py

@@ -2,6 +2,7 @@ from alr_envs.classic_control.hole_reacher import HoleReacher
 from alr_envs.classic_control.viapoint_reacher import ViaPointReacher
 from alr_envs.utils.dmp_env_wrapper import DmpEnvWrapper
 from alr_envs.utils.detpmp_env_wrapper import DetPMPEnvWrapper
+import numpy as np
 
 
 def make_viapointreacher_env(rank, seed=0):
@@ -54,7 +55,7 @@ def make_holereacher_env(rank, seed=0):
                            hole_width=0.15,
                            hole_depth=1,
                            hole_x=1,
-                           collision_penalty=1000)
+                           collision_penalty=100)
 
         _env = DmpEnvWrapper(_env,
                              num_dof=5,
@@ -62,10 +63,51 @@ def make_holereacher_env(rank, seed=0):
                              duration=2,
                              dt=_env.dt,
                              learn_goal=True,
-                             alpha_phase=2,
+                             alpha_phase=3.5,
                              start_pos=_env.start_pos,
                              policy_type="velocity",
                              weights_scale=100,
+                             goal_scale=0.1
+                             )
+
+        _env.seed(seed + rank)
+        return _env
+
+    return _init
+
+
+def make_holereacher_fix_goal_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 = HoleReacher(num_links=5,
+                           allow_self_collision=False,
+                           allow_wall_collision=False,
+                           hole_width=0.15,
+                           hole_depth=1,
+                           hole_x=1,
+                           collision_penalty=100)
+
+        _env = DmpEnvWrapper(_env,
+                             num_dof=5,
+                             num_basis=5,
+                             duration=2,
+                             dt=_env.dt,
+                             learn_goal=False,
+                             final_pos=np.array([2.02669572, -1.25966385, -1.51618198, -0.80946476,  0.02012344]),
+                             alpha_phase=3.5,
+                             start_pos=_env.start_pos,
+                             policy_type="velocity",
+                             weights_scale=50,
+                             goal_scale=1
                              )
 
         _env.seed(seed + rank)
@@ -103,7 +145,7 @@ def make_holereacher_env_pmp(rank, seed=0):
                                 duration=2,
                                 post_traj_time=0,
                                 dt=_env.dt,
-                                weights_scale=0.15,
+                                weights_scale=0.25,
                                 zero_start=True,
                                 zero_goal=False
                                 )