def NRM_dir_stat(sc):
"""
Function to calculate the NRM direction statistics, DANG, free floating direction and NRMdev.
input: Mdec_free,Minc_free, x, y, z, Yint
output: DANG, NRMdev, dir_vec_free
"""
# input: directional_statistics/ mean_dir_stat[Mdec_free,Minc_free, x, y, z]
# arai_statistics/ intercept_stats[Yint]
# output: directional_statistics/ NRM_dir_stat[DANG, NRMdev, dir_vec_free]
x = sc["directional_statistics"]["mean_dir_stat"]["x"]
y = sc["directional_statistics"]["mean_dir_stat"]["y"]
z = sc["directional_statistics"]["mean_dir_stat"]["z"]
Mdec_free = sc["directional_statistics"]["mean_dir_stat"]["Mdec_free"]
Minc_free = sc["directional_statistics"]["mean_dir_stat"]["Minc_free"]
Yint = sc["arai_statistics"]["intercept_stats"]["Yint"]
mean_x = sum(x) / len(x)
mean_y = sum(y) / len(y)
mean_z = sum(z) / len(z)
center_of_mass = [mean_x, mean_y, mean_z]
# calculate DANG & NRMdev
dir_vec_free = helpers.dir2cart(Mdec_free, Minc_free, 1)
DANG = helpers.get_angle_diff(dir_vec_free, center_of_mass)
NRMdev = ((math.sin(math.radians(DANG)) * helpers.norm(center_of_mass)) / abs(Yint)) * 100
sc["directional_statistics"]["NRM_dir_stat"]["dir_vec_free"] = dir_vec_free
sc["directional_statistics"]["NRM_dir_stat"]["DANG"] = DANG
sc["directional_statistics"]["NRM_dir_stat"]["NRMdev"] = NRMdev
return sc
def theta_stat(sc):
"""
Function to calculate the theta statistic, the angle between the applied field direction and the ChRM direction of the NRM
input: free floating direction of best-fit selection (dir_vec_free), and the applied field direction vector (field_dir_vec)
output: the angle between the two (theta)
"""
# input: directional_statistics/ NRM_dir_stat[dir_vec_free]
# preprocessed/ field_basics[field_dir_vec]
# output: directional_statistics/ theta_stat[theta]
dir_vec_free = sc["directional_statistics"]["NRM_dir_stat"]["dir_vec_free"]
field_dir_vec = sc["preprocessed"]["field_basics"]["field_dir_vec"]
theta = helpers.get_angle_diff(dir_vec_free, field_dir_vec)
sc["directional_statistics"]["theta_stat"]["theta"] = theta
return sc
def gamma_stat(sc):
"""
Function to calculate the gamma statistic, The angle between the pTRM acquired at the last step used for the best-fit segment and the applied field direction (BLab)
input: pTRM vector (ptrm), and the applied field direction vector (field_dir_vec)
output: paleointensity estimate (B_anc)
"""
# input: preprocessed/ basics[ptrm]
# field_basics[field_dir_vec]
# output: directional_statistics/ gamma_stat[gamma]
ptrm = sc["preprocessed"]["basics"]["ptrm"]
field_dir_vec = sc["preprocessed"]["field_basics"]["field_dir_vec"]
ptrm_last_step = []
ptrm_last_step = [ptrm[len(ptrm) - 1]["x"], ptrm[len(ptrm) - 1]["y"], ptrm[len(ptrm) - 1]["z"]]
gamma = helpers.get_angle_diff(ptrm_last_step, field_dir_vec)
sc["directional_statistics"]["gamma_stat"]["gamma"] = gamma
return sc
def alpha_stat(sc):
"""
Function to calculate the alpha statistic, the angular difference between the anchored and free-floating best-fit directions
input: free floating direction vector (dir_vec_free), and the anchored direction (Mdec_anc, Minc_anc)
output: alpha
"""
# input: directional_statistics/ mean_dir_stat[Mdec_anc, Minc_anc]
# NRM_dir_stat[dir_vec_free]
# output: directional_statistics/ alpha_stat[alpha]
Mdec_anc = sc["directional_statistics"]["mean_dir_stat"]["Mdec_anc"]
Minc_anc = sc["directional_statistics"]["mean_dir_stat"]["Minc_anc"]
dir_vec_free = sc["directional_statistics"]["NRM_dir_stat"]["dir_vec_free"]
dir_vec_anc = []
dir_vec_anc = helpers.dir2cart(Mdec_anc, Minc_anc, 1)
alpha = helpers.get_angle_diff(dir_vec_free, dir_vec_anc)
sc["directional_statistics"]["alpha_stat"]["alpha"] = alpha
return sc
s.clientID,
objectHandle=s.handles["robot_handle"],
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_streaming)
time.sleep(0.5)
errorCode = 0
while (errorCode == 0):
errorCode, euler_angles = vrep.simxGetObjectOrientation(
s.clientID,
objectHandle=s.handles["robot_handle"],
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_buffer)
errorCode, robo_pos = vrep.simxGetObjectPosition(
s.clientID,
objectHandle=s.handles["robot_handle"],
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_buffer)
angle_a = np.deg2rad * path_array[10, 5]
angle_b = euler_angles[2]
print("angles: {},{}, diff: {}, robo_pos: {}".format(
angle_a, angle_b, helpers.get_angle_diff(angle_a, angle_b), 0))
# print(euler_angles)
dist, i = helpers.get_dist_to_path(robo_pos, path_array[:, :3])
print("dist_to_path: {}, closest_path_point: {}".format(dist, i))
time.sleep(2)
path = pandas.read_csv('./path1.csv', ',')
path_array = path.as_matrix()
path_array = path_array[:, :6]
def _self_observe(self):
global VEHICLE_NAME
X_AXIS_POS = 0
Y_AXIS_POS = 1
Z_AXIS_POS = 2
X_AXIS_ANGLE = 3
Y_AXIS_ANGLE = 4
Z_AXIS_ANGLE = 5
# observe
self.car_pos = np.array(self.car.get_position())
car_angle = self.car.get_orientation()[2]
# print('Car_angle: {}'.format(car_angle))
end_pos = np.array(self.end_point.get_position())
self.dist_to_target = np.linalg.norm(end_pos - self.car_pos)
self.cross_track_err, closest_point_index = helpers.get_dist_to_path(
self.car_pos, self.path[:, :3])
self.path_progress = float(closest_point_index) / self.n_path_elements[
self.current_path]
# print("path pos: ")
# print(self.path[closest_point_index,:3])
# Give crosstrack error the right sign, pos if right, neg if left
vector_path_to_car = np.array(self.car_pos) - np.array(
self.path[closest_point_index, :3])
if closest_point_index == 0:
vector_path_direction = np.array(self.path[closest_point_index + 1, :3]) - \
np.array(self.path[closest_point_index, :3])
else:
vector_path_direction = np.array(self.path[closest_point_index, :3]) - \
np.array(self.path[closest_point_index - 1, :3])
cross_prod = np.cross(vector_path_direction, vector_path_to_car)
if cross_prod[2] > 0:
self.cross_track_err *= -1
# angle difference between path direction and car
path_angle = helpers.vectorAngle(vector_path_direction[:2])
self.diff_angle = helpers.get_angle_diff(np.deg2rad(path_angle),
car_angle)
# print("car angle: {}, path angle: {}, diff angle: {}".format(car_angle,
# np.deg2rad(path_angle), self.diff_angle))
if VEHICLE_NAME == "Pioneer_p3dx":
_, r_wheel_vel = self.r_wheel.get_velocity()
r_wheel_vel = r_wheel_vel[2]
_, l_wheel_vel = self.l_wheel.get_velocity()
l_wheel_vel = l_wheel_vel[2]
self.observation = np.array([
self.cross_track_err, self.diff_angle, r_wheel_vel, l_wheel_vel
]).astype('float32')
elif VEHICLE_NAME == "Manta#0":
car_vel, car_ang_vel = self.car.get_velocity()
car_vel = car_vel[0]
car_ang_vel = car_ang_vel[2]
cross_track_vel = np.sin(self.diff_angle) * abs(car_vel)
# print("VELOCITIES: car_vel: {}, vector_path_to_car: {}, cross_track_vel: {}".format(car_vel,
# vector_path_to_car,
# cross_track_vel))
steering_angle = self.steering.get_joint_angle()
steering_angle = steering_angle # around the z-axis
self.observation = np.array([
self.cross_track_err, cross_track_vel, self.diff_angle,
car_ang_vel
]).astype('float32')
def Tail_check_dtstar_stat(sc):
"""
Function to calculate the extent of a pTRM tail after correction for angular dependence (Leonhardt et al., 2004a; 2004b).
input: y_tail_check, y_temp_tail_check, y_nrm, y_nrm_all, x_temp_all, y_temp_all, NRM_vec_select, NRM_vec_all, xy_temp, field_dir_vec, Yint, Xint, b_slope
output: dt_star
"""
# input: preprocessed/ checks[y_tail_check, y_temp_tail_check]
# basics[y_nrm, y_nrm_all, x_temp_all, y_temp_all, NRM_vec_select, NRM_vec_all, xy_temp]
# field_basics[field_dir_vec]
# arai_statistics/ intercept_stats[Yint, Xint]
# PI_est[b_slope]
# output: check_statistics/ Tail_check_stat[dt_star]
tail_check_vec = sc["preprocessed"]["checks"]["tail_check_vec"]
y_temp_tail_check = sc["preprocessed"]["checks"]["y_temp_tail_check"]
y_nrm = sc["preprocessed"]["basics"]["y_nrm"]
y_nrm_all = sc["preprocessed"]["basics"]["y_nrm_all"]
x_temp_all = sc["preprocessed"]["basics"]["x_temp_all"]
y_temp_all = sc["preprocessed"]["basics"]["y_temp_all"]
xy_temp = sc["preprocessed"]["basics"]["xy_temp"]
NRM_vec_select = sc["preprocessed"]["basics"]["NRM_vec_select"]
NRM_vec_all = sc["preprocessed"]["basics"]["NRM_vec_all"]
field_dir_vec = sc["preprocessed"]["field_basics"]["field_dir_vec"]
b_slope = sc["arai_statistics"]["PI_est"]["b_slope"]
Yint = sc["arai_statistics"]["intercept_stats"]["Yint"]
Xint = sc["arai_statistics"]["intercept_stats"]["Xint"]
dt_star = []
t_star = []
if len(y_temp_tail_check
) != 0: # then you have at least one tailcheck, do calculations
# make an index matrix for the steps that step tail = step nrm_all
ind = []
for i in range(len(y_temp_tail_check)): # number of tail checks
for j in range(len(y_temp_all)): # loop over all NRM steps
if y_temp_tail_check[i] == y_temp_all[
j]: # if step of tail check equals step op NRM, write index to "ind" this is used below
ind.append(j)
# do calculations per tail check step
for i in range(len(y_temp_tail_check)):
MDx = tail_check_vec[i][0]
MDy = tail_check_vec[i][1]
MDz = tail_check_vec[i][2]
Nx = NRM_vec_all[ind[i]][
0] # take the NRM vector with same step as tail_check
Ny = NRM_vec_all[ind[i]][1]
Nz = NRM_vec_all[ind[i]][2]
a = helpers.list_div_num(NRM_vec_all[ind[i]], y_nrm_all[
ind[i]]) # divide the NRM list by the corresponding moment
theta_dt = helpers.get_angle_diff(
a, field_dir_vec
) # get angle between nomalized NRM vector and field direction
# calculate dH and dZ and inc_diff
# according to orientation of Blab
if abs(field_dir_vec[0] /
helpers.norm(field_dir_vec)) == 1: # then allong x
dH = math.sqrt(Ny**2 + Nz**2) - math.sqrt(MDy**2 + MDz**2)
dZ = Nx - MDx
Dec_F, Inc_F, R_F = helpers.cart2dir(field_dir_vec[0],
field_dir_vec[1],
field_dir_vec[2])
Dec_N, Inc_N, R_N = helpers.cart2dir(Nx, Ny, Nz)
inc_diff = Inc_F - Inc_N
elif abs(field_dir_vec[1] /
helpers.norm(field_dir_vec)) == 1: # then allong y
dH = math.sqrt(Nx**2 + Nz**2) - math.sqrt(MDx**2 + MDz**2)
dZ = Ny - MDy
Dec_F, Inc_F, R_F = helpers.cart2dir(field_dir_vec[0],
field_dir_vec[1],
field_dir_vec[2])
Dec_N, Inc_N, R_N = helpers.cart2dir(Nx, Ny, Nz)
inc_diff = Inc_F - Inc_N
elif abs(field_dir_vec[2] /
helpers.norm(field_dir_vec)) == 1: # then allong z
dH = math.sqrt(Nx**2 + Ny**2) - math.sqrt(MDx**2 + MDy**2)
dZ = Nz - MDz
Dec_F, Inc_F, R_F = helpers.cart2dir(field_dir_vec[0],
field_dir_vec[1],
field_dir_vec[2])
Dec_N, Inc_N, R_N = helpers.cart2dir(Nx, Ny, Nz)
inc_diff = Inc_F - Inc_N
else: # labfiels is not along any of the principal axis, do not calulate dt*
dH = 9999
if dH != 9999: # if it is, the labfield is not allong pinciple axix and no calculations are there
B = dH / math.tan(theta_dt)
Lim_upp = 2968 # rad *1000 -> 2.968 rad = 170 degrees
Lim_low = 175 # rad *1000 -> 0.175 rad = 10 degrees
# check if data is within limits
# floor rounds to the nearest small integer
if (math.floor(theta_dt * 1000)) < Lim_upp and (math.floor(
theta_dt * 1000)) > Lim_low:
if inc_diff > 0:
t_star.append(
(-dZ + B) * abs(b_slope) * 100 / abs(Yint))
else:
t_star.append(
(dZ - B) * abs(b_slope) * 100 / abs(Yint))
if (math.floor(theta_dt * 1000)) < Lim_low:
t_star.append(0)
if (math.floor(theta_dt * 1000)) > Lim_upp:
t_star.append(-dZ * 100 / (abs(Yint) + abs(Xint)))
if max(t_star) > 0:
dt_star = max(t_star)
else:
dt_star = 0
sc["check_statistics"]["Tail_check_stat"]["dt_star"] = dt_star
return sc