get_path_to_target

scripts/gyms/Sprint.py

@@ -232,10 +232,10 @@ class sprint(gym.Env):
         t = w.time_local_ms
         self.reset_time = t
 
-        self.target = np.array([3, 0])
+        self.target = np.array([10, 0])
         self.walk_rel_target = self.path_manager.get_path_to_target(target=self.target)[0]
         self.walk_distance = self.path_manager.get_path_to_target(target=self.target)[2]
-        self.walk_rel_orientation = self.path_manager.get_path_to_target(target=self.target)[1]
+        self.walk_rel_orientation = self.path_manager.get_path_to_target(target=self.target)[1] - r.imu_torso_orientation
 
         for _ in range(25):
             self.player.scom.unofficial_beam(self.Gen_player_pos, 0)  # beam player continuously (floating above ground)
@@ -302,7 +302,7 @@ class sprint(gym.Env):
         # self.walk_rel_orientation = M.vector_angle(self.walk_rel_target) * 0.5
         self.walk_rel_target = self.path_manager.get_path_to_target(target=self.target)[0]
         self.walk_distance = self.path_manager.get_path_to_target(target=self.target)[2]
-        self.walk_rel_orientation = self.path_manager.get_path_to_target(target=self.target)[1]
+        self.walk_rel_orientation = self.path_manager.get_path_to_target(target=self.target)[1] - r.imu_torso_orientation
         # exponential moving average
         self.act = 0.8 * self.act + 0.2 * action * action_mult * 0.7
 
@@ -334,14 +334,9 @@ class sprint(gym.Env):
         self.sync()
         self.step_counter += 1
         obs = self.observe()
-        unit_vector = (self.walk_rel_target - r.loc_head_position[:2]) / np.linalg.norm(self.walk_rel_target - r.loc_head_position[:2])
-        if np.linalg.norm(r.loc_torso_velocity[:2]) != 0:
-            cos_theta = np.dot(unit_vector, r.loc_torso_velocity[:2]) / (
-                    np.linalg.norm(unit_vector) * np.linalg.norm(r.loc_torso_velocity[:2]))
-        else:
-            cos_theta = 0
-        reward = np.linalg.norm(r.loc_torso_velocity[:2]) * cos_theta * 0.2
-
+        direction_error = abs(self.walk_rel_orientation)
+        direction_error = min(direction_error, 10)
+        reward = np.linalg.norm(r.loc_torso_velocity[:2])**2 * (1 - direction_error/10) * 0.1
         if np.linalg.norm(self.target - r.loc_head_position[:2]) < 0.3:
             reward += 50
             self.generate_random_target(self.target)