        # get rl_state
        self.p.rl_state[self.p.deta_x_index] = abs(self.a.state[0, -1] - self.w.goal[0, -1])
        self.p.rl_state[self.p.deta_y_index] = abs(self.a.state[1, -1] - self.w.goal[1, -1])
        self.p.rl_state[self.p.v_x_index] = self.a.state[4, -1] * np.cos(self.a.state[2, -1])
        self.p.rl_state[self.p.v_y_index] = self.a.state[4, -1] * np.sin(self.a.state[2, -1])
        self.p.rl_state[self.p.heading_index] = np.arccos(np.dot(self.a.state[0:2, -1], self.w.goal[0:2, -1]) / \