From 2fd1f8c3ef3548b1bc322dc315a2c10a11942017 Mon Sep 17 00:00:00 2001 From: MagDish <2717360869@qq.com> Date: Sat, 16 Nov 2024 17:17:44 +0800 Subject: [PATCH] get_path_to_target --- scripts/gyms/Sprint.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/scripts/gyms/Sprint.py b/scripts/gyms/Sprint.py index bbc70ae..13a396d 100644 --- a/scripts/gyms/Sprint.py +++ b/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)