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)
