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