def _robot_server_state_to_env_state(self, rs_state):
"""Transform state from Robot Server to environment format.
Args:
rs_state (list): State in Robot Server format.
Returns:
numpy.array: State in environment format.
"""
# Convert to numpy array and remove NaN values
rs_state = np.nan_to_num(np.array(rs_state))
# Transform cartesian coordinates of target to polar coordinates
polar_r, polar_theta = utils.cartesian_to_polar_2d(x_target=rs_state[0],\
y_target=rs_state[1],\
x_origin=rs_state[3],\
y_origin=rs_state[4])
# Rotate origin of polar coordinates frame to be matching with robot frame and normalize to +/- pi
polar_theta = utils.normalize_angle_rad(polar_theta - rs_state[5])
# Get Laser scanners data
raw_laser_scan = rs_state[8:1020]
# Downsampling of laser values by picking every n-th value
if self.laser_len > 0:
laser = utils.downsample_list_to_len(raw_laser_scan,self.laser_len)
# Compose environment state
state = np.concatenate((np.array([polar_r, polar_theta]),rs_state[6:8],laser))
else:
# Compose environment state
state = np.concatenate((np.array([polar_r, polar_theta]),rs_state[6:8]))
return state
def test_cartesian_to_polar_2d_default_origin():
target_x = 3
target_y = 4
polar_r, polar_theta = utils.cartesian_to_polar_2d(x_target=target_x, y_target=target_y)
assert abs(polar_r - 5) < 0.01
assert abs(polar_theta - 0.927) < 0.01
def test_cartesian_to_polar_2d_equal_points(p1, p2, expected_r,
expected_theta):
polar_r, polar_theta = utils.cartesian_to_polar_2d(x_target=p1[0],
y_target=p1[1],
x_origin=p2[0],
y_origin=p2[1])
assert abs(polar_r - expected_r) < 0.001
assert abs(polar_theta - expected_theta) < 0.001
def test_cartesian_to_polar_2d_set_origin():
target_x = 3
target_y = 4
origin_x = 1
origin_y = 1
polar_r, polar_theta = utils.cartesian_to_polar_2d(x_target=target_x, y_target=target_y, x_origin=origin_x, y_origin=origin_y)
assert abs(polar_r - 3.61) < 0.01
assert abs(polar_theta - 0.9827949) < 0.01
