def get_obs_dict(self): """Returns the current observation of the environment. Returns: A dictionary of observation values. This should be an ordered dictionary if `observation_keys` isn't set. """ robot_state = self.robot.get_state('dkitty') target_state, heading_state, torso_track_state = self.tracker.get_state( ['target', 'heading', 'torso']) #target_xy = target_state.pos[:2] target_xy = np.array([0, 2 * self._goal_dir]) kitty_xy = torso_track_state.pos[:2] # Get the heading of the torso (the y-axis). current_heading = torso_track_state.rot[:2, 1] # Get the direction towards the heading location. #heading_xy = heading_state.pos[:2] heading_xy = np.array([0, 2]) desired_heading = heading_xy - kitty_xy # Calculate the alignment of the heading with the desired direction. heading = calculate_cosine(current_heading, desired_heading) return collections.OrderedDict(( # Add observation terms relating to being upright. *self._get_upright_obs(torso_track_state).items(), ('root_pos', torso_track_state.pos), ('root_euler', torso_track_state.rot_euler), ('root_vel', torso_track_state.vel), ('root_angular_vel', torso_track_state.angular_vel), ('kitty_qpos', robot_state.qpos), ('kitty_qvel', robot_state.qvel), ('last_action', self._get_last_action()), ('heading', heading), ('target_pos', target_xy), ('target_error', target_xy - kitty_xy), ))
def get_obs_dict(self) -> Dict[str, np.ndarray]: """Returns the current observation of the environment. Returns: A dictionary of observation values. This should be an ordered dictionary if `observation_keys` isn't set. """ robot_state = self.robot.get_state('dkitty') torso_track_state, target_track_state = self.tracker.get_state( ['torso', 'target']) # Get the facing direction of the kitty. (the y-axis). current_facing = torso_track_state.rot[:2, 1] # Get the direction to the target. target_facing = target_track_state.pos[:2] - torso_track_state.pos[:2] target_facing = target_facing / np.linalg.norm(target_facing + 1e-8) # Calculate the alignment to the facing direction. angle_error = np.arccos(calculate_cosine(current_facing, target_facing)) self._update_markers(torso_track_state.pos, current_facing, target_facing) return collections.OrderedDict(( # Add observation terms relating to being upright. *self._get_upright_obs(torso_track_state).items(), ('root_pos', torso_track_state.pos), ('root_euler', torso_track_state.rot_euler), ('root_vel', torso_track_state.vel), ('root_angular_vel', torso_track_state.angular_vel), ('kitty_qpos', robot_state.qpos), ('kitty_qvel', robot_state.qvel), ('last_action', self._get_last_action()), ('current_facing', current_facing), ('desired_facing', target_facing), ('angle_error', angle_error), ))
def test_zero(self): """Tests when the norm is 0.""" v1 = np.array([1, 0]) v2 = np.array([0, 0]) self.assertAlmostEqual(calculate_cosine(v1, v2), 0)
def test_zero_batched(self): """Tests when the norm is 0.""" v1 = np.array([[1, 0], [1, 1]]) v2 = np.array([[0, 0], [2, 2]]) np.testing.assert_array_almost_equal(calculate_cosine(v1, v2), [0, 1])
def test_batched(self): """Multiple vectors.""" v1 = np.array([[1, 1], [2, 2]]) v2 = np.array([[1, -1], [3, 3]]) np.testing.assert_array_almost_equal(calculate_cosine(v1, v2), [0, 1])
def test_orthogonal(self): """Two orthogonal vectors.""" v1 = np.array([1, 1]) v2 = np.array([1, -1]) self.assertAlmostEqual(calculate_cosine(v1, v2), 0)
def test_opposite(self): """Two parallel vectors.""" v1 = np.array([1, 2]) v2 = np.array([-1, -2]) self.assertAlmostEqual(calculate_cosine(v1, v2), -1)
def test_parallel(self): """Two parallel vectors.""" v1 = np.array([1, 2]) v2 = np.array([2, 4]) self.assertAlmostEqual(calculate_cosine(v1, v2), 1)
def test_identical(self): """Two of the same vectors are completely aligned.""" v1 = np.array([1, 0]) self.assertAlmostEqual(calculate_cosine(v1, v1), 1)