def calc_vel(AA, q, Yd, Conn, Prop): """ Returns the linear velocity of all body centroids and the angular velocity of all body (eqs. 3.8-3.9 and 3.12-3.13, figure 3.5). vv = [vv_0, vv_1, ... ] (3, num_b) ww = [ww_0, ww_1, ... ] (3, num_b) """ BB, j_type = Conn[2], Conn[3] cc = Prop[2] v0, w0, qd = Yd[0:3], Yd[3:6], Yd[6:] num_b = len(q) + 1 # Number of bodies vv = np.zeros((3, num_b)) ww = np.zeros((3, num_b)) # Base vv[:, 0] = v0 ww[:, 0] = w0 # i = current link, k = lower link connection for i in range(1, num_b): idxi = i - 1 # Index link/joint <i> in BB, j_type, q, qd k = BB[idxi] # Index lower link connection # Rotation matrices A_I_i = AA[:, 3 * i:3 * (i + 1)] A_I_k = AA[:, 3 * k:3 * (k + 1)] # Relative positions cc_I_i = A_I_i @ cc[:, i, i] cc_I_k = A_I_k @ cc[:, k, i] # Joint values q_I_i = A_I_i @ (Ez * q[idxi]) qd_I_i = A_I_i @ (Ez * qd[idxi]) # Rotational joint (eqs. 3.8-3.9) if (j_type[idxi] == 'R'): ww[:, i] = ww[:, k] + qd_I_i vv[:, i] = vv[:, k] + cross(ww[:, k], cc_I_k) \ - cross(ww[:, i], cc_I_i) # Prismatic joint (eqs. 3.12-3.13) elif (j_type[idxi] == 'P'): ww[:, i] = ww[:, k] vv[:, i] = vv[:, k] + cross(ww[:, k], cc_I_k) \ - cross(ww[:, i], cc_I_i) \ + cross(ww[:, i], q_I_i) \ + qd_I_i return vv, ww
def side(self, v0, v1, v2, origin, direction): v0v1 = sub(v1, v0) v0v2 = sub(v2, v0) N = cross(v0v1, v0v2) raydirection = dot(N, direction) if abs(raydirection) < 0.0001: return None d = dot(N, v0) t = (dot(N, origin) + d) / raydirection if t < 0: return None P = sum(origin, mul(direction, t)) U, V, W = barycentric(v0, v1, v2, P) if U < 0 or V < 0 or W < 0: return None else: return Intersect(distance = d, point = P, normal = norm(N))
def __init__(self, point_item): self.vertices = set() if not point_item.is_bounded(): return # Compute segments edges for other in point_item.others: v1, v2 = other.vertices() for v in [v1, v2]: EPSILON = 1e-4 distances = list(utils.distance2(v, i) for i in self.vertices) if len(distances) == 0 or min(distances) > EPSILON: self.vertices |= {v} # Sort vertices to make the shape convex self.vertices = list(self.vertices) OA = utils.vector(point_item.point, self.vertices[0]) len_OA = utils.length(OA) t = {(self.vertices[0], 0)} for vertex in self.vertices[1:]: OB = utils.vector(point_item.point, vertex) len_OB = utils.length(OB) cos_angle = utils.dot(OA, OB) / (len_OA * len_OB) if cos_angle < -1: cos_angle = -1 elif cos_angle > 1: cos_angle = 1 angle = math.acos(cos_angle) if utils.cross(OA, OB) < 0: # sign of sin angle = 2 * math.pi - angle t |= {(vertex, angle)} self.vertices = list(vertex for vertex, _ in sorted(t, key=lambda x: x[1]))
def take_RK4_step(self, b_rand): o, c = self.options, self.constants # Get the current position p = self.current_position() # Calculate partial Runge Kutta steps k1 = self.calculate_function_value(p) k2 = self.calculate_function_value( p + k1 * o['dt'] / 2) # Needs B_eff with spins p+k1 k3 = self.calculate_function_value(p + k2 * o['dt'] / 2) k4 = self.calculate_function_value(p + k3 * o['dt']) # Calculate the total difference in the spin d_spin = (k1 + 2 * k2 + 2 * k3 + k4) * o['dt'] / 6 # Calculate the random energy added # This is based on temperature d_spin_rand = c['gamma'] * cross(p, b_rand) # Calculate new position and normalise the vector p += d_spin + d_spin_rand p = p / math.sqrt(dot(p, p)) # Save the data to the atom self.set_position(p) return self.id, p
def generate_submit_constrained_parameters_grid(self, dict_parameters_range, filtering_function=None, filtering_function_parameters=None, pbs_submission_infos=None, submit_jobs=True): ''' Takes a dictionary of parameters, with their list of values, and generates a list of all the combinations. Filter them with a specific function if provided. if pbs_submission_infos is provided, will create a script and submit it to PBS when an acceptable set of parameters if found ''' candidate_parameters = [] # Get all cross combinations of parameters cross_comb = cross([dict_parameters_range[param]['range'].tolist() for param in dict_parameters_range]) # Convert them back into dictionaries candidate_parameters = [dict(zip(dict_parameters_range.keys(), x)) for x in cross_comb] # Now filter them constrained_parameters = [] for new_parameters in progress.ProgressDisplay(candidate_parameters, display=progress.SINGLE_LINE): if (filtering_function and filtering_function(new_parameters, dict_parameters_range, filtering_function_parameters)) or (filtering_function is None): constrained_parameters.append(new_parameters) # Submit to PBS if required if pbs_submission_infos: self.create_submit_job_parameters(pbs_submission_infos, new_parameters, submit=submit_jobs) return constrained_parameters
def take_RK2_step(self, b_rand): o, c = self.options, self.constants # Get the current position p = self.current_position() # Calculate partial Runge Kutta steps k1 = self.calculate_function_value(p) self.set_position(p + k1 * o['dt']) # self.combine_neighbours # We need to call this, but then we have to call from particles k2 = self.calculate_function_value(self.pos) # Calculate the total difference in the spin # d_spin = (k1 + k2)/2*o['dt'] d_spin = k2 * o['dt'] # Calculate the random energy added # This is based on temperature d_spin_rand = c['gamma'] * cross(p, b_rand) # Calculate new position and normalise the vector p += d_spin + d_spin_rand p = p / math.sqrt(dot(p, p)) # Save the data to the atom self.set_position(p) return self.id, p
def get_mean_activity(self, axes=(0, 1), precision=100, specific_neurons=None, return_axes_vect=False): ''' Returns the mean activity of the network. ''' coverage_1D = self.init_feature_space(precision) possible_stimuli = np.array(utils.cross(self.R * [coverage_1D.tolist()])) activity = np.empty((possible_stimuli.shape[0], self.M)) for stimulus_i, stimulus in enumerate(possible_stimuli): activity[stimulus_i] = self.get_network_response( stimulus, specific_neurons=specific_neurons) # Reshape activity.shape = self.R * (precision, ) + (self.M, ) mean_activity = activity for dim_to_avg in (set(range(len(activity.shape))) - set(axes)): mean_activity = np.mean(mean_activity, axis=dim_to_avg, keepdims=True) mean_activity = np.squeeze(mean_activity) if return_axes_vect: return (mean_activity, coverage_1D, coverage_1D) else: return mean_activity
def take_ad_bs_step(self, b_rand): # Grab the constants o, c = self.options, self.constants pos = self.current_position() # Gradually increase steps in the Adams Bashforth method if self.spos4 is None: d_spos = self.first_ad_bs_step(pos) d_fpos = d_spos elif self.spos3 is None: d_spos, d_fpos = self.second_ad_bs_step(pos) elif self.spos2 is None: d_spos, d_fpos = self.third_ad_bs_step(pos) elif self.spos1 is None: d_spos, d_fpos = self.fourth_ad_bs_step(pos) else: d_spos, d_fpos = self.fifth_ad_bs_step(pos) # Move the values along so we keep continuing self.spos4, self.spos3, self.spos2, self.spos1 = d_fpos, self.spos4, self.spos3, self.spos2 # Calculate the random energy added # This is based on temperature d_spin_rand = c['gamma'] * cross(pos, b_rand) # Take the step and calculate the final position and normalise the vector pos = pos + d_spos * o['dt'] + d_spin_rand pos = pos / math.sqrt(dot(pos, pos)) # Save the data to the atom self.set_position(pos) return self.id, pos
def calc_ang_mom1(self, P_ref=np.zeros(3), V_ref=np.zeros(3)): """ Returns the derivative of the angular momentum for the entire system and for each body with respect to point P_ref and velocity V_ref. """ mass = self.Prop[0] inertia = self.Prop[1] HM = np.zeros((3, self.num_b)) for i in range(self.num_b): AA = self.AA[:, 3 * i:3 * (i + 1)] In = AA @ inertia[:, 3 * i:3 * (i + 1)] @ AA.T HM[:, i] = In @ self.wd[:, i] \ + cross(self.ww[:, i], In @ self.ww[:, i]) \ + cross(self.RR[:, i] - P_ref, mass[i] * self.vd[:, i]) \ + cross(self.vv[:, i] - V_ref, mass[i] * self.vv[:, i]) return HM.sum(axis=1), HM
def __init__(self): """Constructor of the Algorithm Norvig that initialize the variables used in the resolution of a Sudoku """ self.digit_list = '123456789' self.row_list = 'ABCDEFGHI' self.column_list = self.digit_list self.list_squares = utils.cross(self.row_list, self.column_list) self.unit_list = ( [utils.cross(self.row_list, column) for column in self.column_list] + [utils.cross(row, self.column_list) for row in self.row_list] + [utils.cross(rows, columns) for rows in ('ABC', 'DEF', 'GHI') for columns in ('123', '456', '789')]) self.unit_dict_squares = dict( (square, [unit for unit in self.unit_list if square in unit]) for square in self.list_squares) self.peers_set_squares = dict((square, set( sum(self.unit_dict_squares[square], [])) - set([square])) for square in self.list_squares)
def __init__(self): """Constructor of the Algorithm Norvig that initialize the variables used in the resolution of a Sudoku """ self.digit_list = '123456789' self.row_list = 'ABCDEFGHI' self.column_list = self.digit_list self.list_squares = utils.cross(self.row_list, self.column_list) self.unit_list = ([ utils.cross(self.row_list, column) for column in self.column_list ] + [utils.cross(row, self.column_list) for row in self.row_list] + [ utils.cross(rows, columns) for rows in ('ABC', 'DEF', 'GHI') for columns in ('123', '456', '789') ]) self.unit_dict_squares = dict( (square, [unit for unit in self.unit_list if square in unit]) for square in self.list_squares) self.peers_set_squares = dict( (square, set(sum(self.unit_dict_squares[square], [])) - set([square])) for square in self.list_squares)
def test_verify_that_the_cross_method_generate_the_peers(self): self.cols = '123456789' self.rows = 'ABCDEFGHI' expected_peers = ['A1', 'A2', 'A3', 'A4', 'A5', 'A6', 'A7', 'A8', 'A9', 'B1', 'B2', 'B3', 'B4', 'B5', 'B6', 'B7', 'B8', 'B9', 'C1', 'C2', 'C3', 'C4', 'C5', 'C6', 'C7', 'C8', 'C9', 'D1', 'D2', 'D3', 'D4', 'D5', 'D6', 'D7', 'D8', 'D9', 'E1', 'E2', 'E3', 'E4', 'E5', 'E6', 'E7', 'E8', 'E9', 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', 'F7', 'F8', 'F9', 'G1', 'G2', 'G3', 'G4', 'G5', 'G6', 'G7', 'G8', 'G9', 'H1', 'H2', 'H3', 'H4', 'H5', 'H6', 'H7', 'H8', 'H9', 'I1', 'I2', 'I3', 'I4', 'I5', 'I6', 'I7', 'I8', 'I9'] self.assertEqual(expected_peers, utils.cross(self.rows, self.cols))
def test_verify_that_the_cross_method_generate_the_peers(self): self.cols = '123456789' self.rows = 'ABCDEFGHI' expected_peers = [ 'A1', 'A2', 'A3', 'A4', 'A5', 'A6', 'A7', 'A8', 'A9', 'B1', 'B2', 'B3', 'B4', 'B5', 'B6', 'B7', 'B8', 'B9', 'C1', 'C2', 'C3', 'C4', 'C5', 'C6', 'C7', 'C8', 'C9', 'D1', 'D2', 'D3', 'D4', 'D5', 'D6', 'D7', 'D8', 'D9', 'E1', 'E2', 'E3', 'E4', 'E5', 'E6', 'E7', 'E8', 'E9', 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', 'F7', 'F8', 'F9', 'G1', 'G2', 'G3', 'G4', 'G5', 'G6', 'G7', 'G8', 'G9', 'H1', 'H2', 'H3', 'H4', 'H5', 'H6', 'H7', 'H8', 'H9', 'I1', 'I2', 'I3', 'I4', 'I5', 'I6', 'I7', 'I8', 'I9' ] self.assertEqual(expected_peers, utils.cross(self.rows, self.cols))
def forward(self, input, logdet=0, reverse=False): if not reverse: x1, x2 = utils.split(input) h = self.Net(x1) t, h_scale = utils.cross(h) scale = torch.nn.functional.softplus(h_scale) logscale = torch.log(scale) y2 = (x2 + t) * scale y1 = x1 y = torch.cat((y1, y2), 1) logdet += utils.flatten_sum(logscale) return y, logdet else: y1, y2 = utils.split(input) h = self.Net(y1) t, h_scale = utils.cross(h) scale = torch.nn.functional.softplus(h_scale) logscale = torch.log(scale) x2 = (y2 / scale) - t x1 = y1 x = torch.cat((x1, x2), 1) logdet += utils.flatten_sum(logscale) return x, logdet
def create_euler_from_vectors(v1, v2): # q = Quaternion.create_quaternion_from_vector_to_vector(v1, v2) v1 = v1.reshape(-1) v2 = v2.reshape(-1) direction = cross(v1, v2) x = direction[0] y = direction[1] z = direction[2] w = np.sqrt((v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2]) * (v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2])) + \ (v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]) # explicit_q = [q.x, q.y, q.z, q.w] euler = euler_from_quaternion([x, y, z, w]) euler = np.array(euler).reshape(3, 1) return euler
def assign_prefered_stimuli_conjunctive(self, neurons_indices): ''' Tile conjunctive neurons along the space of possible angles. ''' # Cover the space uniformly N_sqrt = np.floor(np.power(neurons_indices.size, 1./self.R)) # coverage_1D = np.linspace(-np.pi, np.pi, N_sqrt, endpoint=False) coverage_1D = np.linspace(-np.pi + np.pi/N_sqrt, np.pi + np.pi/N_sqrt, N_sqrt, endpoint=False) new_stim = np.array(utils.cross(self.R*[coverage_1D.tolist()])) # Assign the preferred stimuli # Unintialized neurons will get masked out down there. self.neurons_preferred_stimulus[neurons_indices[:new_stim.shape[0]]] = new_stim
def calc_je(ie, RR, AA, q, Conn, Prop): """ Returns the Jacobian (6 x num_j) of the endpoint <ie> (eqs 3.25 and 3.26). """ SE, BB, j_type = Conn[1], Conn[2], Conn[3] cc, ce = Prop[2], Prop[3] # Link sequence from the base (excluded) to the endpoint seq_link = [] i = SE[0, ie] while (i > 0): seq_link.insert(0, i) i = BB[i - 1] # Position of the endpoint j = seq_link[-1] A_I_j = AA[:, 3 * j:3 * (j + 1)] POS_e = RR[:, j] + A_I_j @ ce[:, ie] # Position for all joints in the link sequence n_links = len(seq_link) POS_jnt = np.zeros((3, n_links)) Jacobian = np.zeros((6, len(q))) for i in range(n_links): j = seq_link[i] # Joint/link number in the sequence idxj = j - 1 # Index link/joint <j> in q, j_type A_I_j = AA[:, 3 * j:3 * (j + 1)] Ez_I_j = A_I_j @ Ez # Position for a rotational joint if (j_type[idxj] == 'R'): POS_jnt[:, i] = RR[:, j] + A_I_j @ cc[:, j, j] JJ_te = cross(Ez_I_j, POS_e - POS_jnt[:, i]) JJ_re = Ez_I_j # Prismatic joint (rotational component already set to zero) elif (j_type[idxj] == 'P'): POS_jnt[:, i] = RR[:, j] + A_I_j @ (cc[:, j, j] - Ez * q[idxj]) JJ_te = Ez_I_j JJ_re = np.zeros(3) # Assemble the endpoint Jacobian in the global Jacobian Jacobian[0:3, j - 1] = JJ_te Jacobian[3:6, j - 1] = JJ_re return Jacobian
def create_quaternion_from_vector_to_vector(cls, v1, v2): """ from two direction vectors, create the quaternion formulas from https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another verified by create axis_angle then convert to quaternion Params: v1, v2: direction vectors, expected 3 by 1 Return: q: Quaternion class """ q = cls() direction = cross(v1.reshape([-1]), v2.reshape([-1])) q.x = direction[0] q.y = direction[1] q.z = direction[2] q.w = np.sqrt((np.linalg.norm(v1)**2) * (np.linalg.norm(v2)**2)) + np.matmul(v1.T, v2).item() q.normalize() return q
def triangle_kernel(points, verts, faces, kernel_height=1.0): n_p = points.shape[0] n_f = faces.shape[0] points_expand = points.unsqueeze(1).expand(-1, n_f, -1) face_normals = utils.face_normals(verts, faces) # Longest edge in each triangle # longest_edge = torch.zeros(n_f, dtype=verts.dtype, device=verts.device) # True if point projects inside of face in plane min_edge_dist = torch.ones( (n_p, n_f), dtype=verts.dtype, device=points.device) * float('inf') for i in range(3): lineA = verts[faces[:, i]] lineB = verts[faces[:, (i + 1) % 3]] # Update longest edge # longest_edge = torch.max(longest_edge, torch.norm(lineA - lineB)) # Edge perp vec e_perp = utils.normalize(utils.cross(face_normals, lineB - lineA)) edge_inside_dist = utils.dot( e_perp.unsqueeze(0).expand(n_p, -1, -1), points_expand - lineA.unsqueeze(0).expand(n_p, -1, -1)) # edge_inside_dist = torch.max(dge_inside_dist, torch.tensor(0., device=verts.device)) min_edge_dist = torch.min(min_edge_dist, edge_inside_dist) # normal distance point_in_face = verts[faces[:, 0]].unsqueeze(0).expand(n_p, -1, -1) normal_face_dist = torch.abs( utils.dot( face_normals.unsqueeze(0).expand(n_p, -1, -1), points_expand - point_in_face)) EPS = 1e-8 k_val = torch.max( torch.tensor(0., device=verts.device), 1. - (normal_face_dist / (min_edge_dist * kernel_height + EPS))) k_val = torch.where(min_edge_dist < 0., torch.zeros_like(k_val), k_val) return k_val
def generate_submit_constrained_parameters_grid( self, dict_parameters_range, filtering_function=None, filtering_function_parameters=None, pbs_submission_infos=None, submit_jobs=True): ''' Takes a dictionary of parameters, with their list of values, and generates a list of all the combinations. Filter them with a specific function if provided. if pbs_submission_infos is provided, will create a script and submit it to PBS when an acceptable set of parameters if found ''' candidate_parameters = [] # Get all cross combinations of parameters cross_comb = cross([ dict_parameters_range[param]['range'].tolist() for param in dict_parameters_range ]) # Convert them back into dictionaries candidate_parameters = [ dict(zip(dict_parameters_range.keys(), x)) for x in cross_comb ] # Now filter them constrained_parameters = [] for new_parameters in progress.ProgressDisplay( candidate_parameters, display=progress.SINGLE_LINE): if (filtering_function and filtering_function( new_parameters, dict_parameters_range, filtering_function_parameters)) or (filtering_function is None): constrained_parameters.append(new_parameters) # Submit to PBS if required if pbs_submission_infos: self.create_submit_job_parameters(pbs_submission_infos, new_parameters, submit=submit_jobs) return constrained_parameters
def calc_jac(RR, AA, Conn, Prop): """ Returns the translational Jacobians (3 x num_j x num_j) of all link centroids (equation 3.25-3.26). """ BB, j_type = Conn[2], Conn[3] cc = Prop[2] num_j = len(j_type) # Number of joints/links JJ_t = np.zeros((3, num_j * num_j)) JJ_r = np.zeros((3, num_j * num_j)) # Loop over all links for i in range(1, num_j + 1): j = i # Initial index from link/joint i to base (itself) # Follow the branch until the base (j = 0) is reached while (j > 0): idxj = j - 1 # Index link/joint <j> in BB, j_type A_I_j = AA[:, 3 * j:3 * (j + 1)] Ez_I_j = A_I_j @ Ez cc_I_j = A_I_j @ cc[:, j, j] col = (i - 1) * num_j + (j - 1) # Rotational joint if (j_type[idxj] == 'R'): JJ_t[:, col] = \ cross(Ez_I_j, RR[:, i] - (RR[:, j] + cc_I_j)) JJ_r[:, col] = Ez_I_j # Prismatic joint elif (j_type[idxj] == 'P'): JJ_t[:, col] = Ez_I_j JJ_r[:, col] = np.zeros(3) j = BB[idxj] # Previous link/joint along the branch return JJ_t, JJ_r
def get_neuron_activity_full(self, neuron_index, precision=100, axes=None): """Returns the activity of a specific neuron over the entire space. """ coverage_1D = utils.init_feature_space(precision) possible_stimuli = np.array(utils.cross(self.R * [coverage_1D.tolist()])) activity = np.empty(possible_stimuli.shape[0]) # Compute the activity of that neuron over the whole space for stimulus_i, stimulus in enumerate(possible_stimuli): activity[stimulus_i] = self.get_neuron_response(neuron_index, stimulus) # Reshape activity.shape = self.R * (precision, ) if axes is not None: for dim_to_avg in set(range(len(activity.shape))) - set(axes): activity = np.mean(activity, axis=dim_to_avg, keepdims=True) activity = np.squeeze(activity) return activity
def turn_left(self): self.body.angular_velocity = 0 t = cross(self.left_turn_point, self.turn_force) self.body.apply_torque(t)
from utils import dot, vecadd, cross a = [2, -1, -2] b = [5, -3, 3] c = [3, -1, -3] if __name__ == "__main__": print("a ", dot(a, cross(b, c))) print("b ", dot(a, vecadd(b, c))) print("cde ", cross(a, vecadd(b, c)))
def calculate_function_value(self, p): o, c, B = self.options, self.constants, self.B_eff return c['gamma'] * (cross(p, B) + o['l'] * (B * dot(p, p) - p * dot(p, B)))
def plots_fitting_experiments_random(data_pbs, generator_module=None): ''' Reload 2D volume runs from PBS and plot them ''' #### SETUP # savefigs = True savedata = True plots_per_T = False plots_interpolate = False # do_relaunch_bestparams_pbs = True colormap = None # or 'cubehelix' plt.rcParams['font.size'] = 16 # #### /SETUP print "Order parameters: ", generator_module.dict_parameters_range.keys() # parameters: ratio_conj, sigmax, T # Extract data result_fitexperiments_flat = np.array(data_pbs.dict_arrays['result_fitexperiments']['results_flat']) result_fitexperiments_all_flat = np.array(data_pbs.dict_arrays['result_fitexperiments_all']['results_flat']) result_parameters_flat = np.array(data_pbs.dict_arrays['result_fitexperiments']['parameters_flat']) # Extract order of datasets experiment_ids = data_pbs.loaded_data['datasets_list'][0]['fitexperiment_parameters']['experiment_ids'] dataio = DataIO(output_folder=generator_module.pbs_submission_infos['simul_out_dir'] + '/outputs/', label='global_' + dataset_infos['save_output_filename']) # Compute some stuff result_fitexperiments_bic_avg = utils.nanmean(result_fitexperiments_flat[:, 0, 0], axis=-1) T_space = np.unique(result_parameters_flat[:, 2]) if plots_per_T: for T in T_space: currT_indices = result_parameters_flat[:, 2] == T utils.contourf_interpolate_data_interactive_maxvalue(result_parameters_flat[currT_indices][..., :2], result_fitexperiments_bic_avg[currT_indices], xlabel='Ratio_conj', ylabel='sigma x', title='BIC, T %d' % T, interpolation_numpoints=200, interpolation_method='nearest', log_scale=False) # Interpolate if plots_interpolate: rcscale_target = 5.0 rbf_interpolator = spint.Rbf(result_parameters_flat[:,0], result_parameters_flat[:,1], result_parameters_flat[:,2], result_parameters_flat[:,3], result_fitexperiments_bic_avg, smooth = 0.0) param1_space = np.linspace(0.01, 1.0, 50) param2_space = np.linspace(0.01, 1.0, 50) param3_space = np.array([rcscale_target]) param4_space = np.array([rcscale_target]) params_crossspace = np.array(utils.cross(param1_space, param2_space, param3_space, param4_space)) interpolated_data = rbf_interpolator(params_crossspace[:, 0], params_crossspace[:, 1], params_crossspace[:, 2], params_crossspace[:, 3]).reshape((param1_space.size, param2_space.size)) # utils.pcolor_2d_data(interpolated_data, param1_space, param2_space, 'ratio', 'sigmax', 'interpolated, fixing rcscales= %.2f' % rcscale_target) points_closeby = ((result_parameters_flat[:, 2] - rcscale_target)**2 + (result_parameters_flat[:, 3] - rcscale_target)**2) < 0.5 plt.figure() plt.imshow(interpolated_data, extent=(param1_space.min(), param1_space.max(), param2_space.min(), param2_space.max())) plt.scatter(result_parameters_flat[points_closeby, 0], result_parameters_flat[points_closeby, 1], s=100, c=result_fitexperiments_bic_avg[points_closeby], marker='o') # if plot_per_ratio: # # Plot the evolution of loglike as a function of sigmax, with std shown # for ratio_conj_i, ratio_conj in enumerate(ratio_space): # ax = utils.plot_mean_std_area(sigmax_space, result_log_posterior_mean[ratio_conj_i], result_log_posterior_std[ratio_conj_i]) # ax.get_figure().canvas.draw() # if savefigs: # dataio.save_current_figure('results_fitexp_%s_loglike_ratioconj%.2f_{label}_global_{unique_id}.pdf' % (exp_dataset, ratio_conj)) all_args = data_pbs.loaded_data['args_list'] variables_to_save = ['experiment_ids'] if savedata: dataio.save_variables_default(locals(), variables_to_save) dataio.make_link_output_to_dropbox(dropbox_current_experiment_folder='fitting_experiments') plt.show() return locals()
for s in st.get_skeleton(1): e = s[0] if len(e) == 2: fle_edges.append(Y[[e[0],e[1]]]) plt.scatter(Y[:,0], Y[:,1],c =t, cmap = plt.cm.Spectral) lc = LineCollection(segments = fle_edges, linewidths=0.3) plt.gca().add_collection(lc) plt.xticks([], []) plt.yticks([], []) if save_fig: plt.savefig(fig_dir + data_choice + '_FLE_1step.pdf', bbox_inches='tight') plt.show() #%% crosses = cross(fle_edges, edge_list, findall=True) if len(crosses) != 0: print('{} cross found!'.format(len(crosses))) else: print('No cross found!') #%% Second-step Fixed-point LE if twostepFLE and (not use_boundary): # detect boundary again because we haven't done so if use_boundary == False boundary_edge, boundary_point_idx = detect_boundary(st, edge_list) # if there is no boundary, we raise an error (note error msg different) assert len(boundary_point_idx) > 0, "No boundary detected, one-step FLE is sufficient"
def keyboard_control(args_, pygame_clock, save_screenshot_path, vehicles_, walkers_, world_, client_): """Process all the keyboard event since last tick.""" # the rgb camera and seg camera are in the args # since we may need to change the fov for them # by rebuild the camera actor # return True to exit ms_since_last_tick = pygame_clock.get_time() # TOdo: change the following. dont get the transform at each time to avoid # stuttering due to the frame lag # set a global variable of the rotation instead prev_rotation = args_.spectator.get_transform().rotation prev_location = args_.spectator.get_transform().location global_up_vector = carla.Vector3D(x=0, z=1, y=0) # a normalized x,y,z, between 0~1 forward_vector = prev_rotation.get_forward_vector() left_vector = cross(forward_vector, global_up_vector) global_forward_vector = cross(global_up_vector, left_vector) # up_vector = cross(left_vector, forward_vector) # get all event from event queue # empty out the event queue for event in pygame.event.get(): if event.type == pygame.QUIT: return True elif event.type == pygame.MOUSEBUTTONUP: # get the depth map in meters depth_in_meters = parse_carla_depth(args_.depth_camera.rgb_image) pos_x, pos_y = pygame.mouse.get_pos() click_point = np.array([pos_x, pos_y, 1]) intrinsic = args_.rgb_camera.camera_actor.intrinsic # 3d point in the camera coordinates click_point_3d = np.dot(np.linalg.inv(intrinsic), click_point) click_point_3d *= depth_in_meters[pos_y, pos_x] # why? this is because unreal transform is (y, -z , x)??? y, z, x = click_point_3d z = -z click_point_3d = np.array([x, y, z, 1]) click_point_3d.reshape([4, 1]) # transform to the world origin camera_rt = compute_extrinsic_from_transform( args_.rgb_camera.camera_actor.get_transform()) click_point_world_3d = np.dot(camera_rt, click_point_3d) x, y, z = click_point_world_3d.tolist( )[0][:3] # since it is np.matrix red = carla.Color(r=255, g=0, b=0) green = carla.Color(r=0, g=255, b=0) blue = carla.Color(r=0, g=0, b=255) if args_.last_click_point is None: args_.last_click_point = carla.Location(x=x, y=y, z=z) print("Click origin location xyz: %s %s %s" % (x, y, z)) world.debug.draw_point(args_.last_click_point, color=red, size=0.3, life_time=30) else: this_click_point = carla.Location(x=x, y=y, z=z) world.debug.draw_arrow(args_.last_click_point, this_click_point, thickness=0.2, arrow_size=0.2, color=red, life_time=30) # draw the (x=1.0, y=0.0) and (x=0.0, y=1.0) point translated to the # origin point, and compute the rotation degree needed ref_point = (1.0 + args_.last_click_point.x, 0 + args_.last_click_point.y) ref_vec = (ref_point[0] - args_.last_click_point.x, ref_point[1] - args_.last_click_point.y) this_vec = (x - args_.last_click_point.x, y - args_.last_click_point.y) angle_degree = get_degree_of_two_vectors(this_vec, ref_vec) # notice here the degree is negative. print("Click second location xyz: %s, Degree between (1, 0) " "and this vector: %s" % ([x, y, z], -angle_degree)) world.debug.draw_arrow(args_.last_click_point, carla.Location(x=ref_point[0], y=ref_point[1], z=z), thickness=0.1, arrow_size=0.1, color=green, life_time=30) ref_point = (0.0 + args_.last_click_point.x, 1.0 + args_.last_click_point.y) ref_vec = (ref_point[0] - args_.last_click_point.x, ref_point[1] - args_.last_click_point.y) world.debug.draw_arrow(args_.last_click_point, carla.Location(x=ref_point[0], y=ref_point[1], z=z), thickness=0.1, arrow_size=0.1, color=blue, life_time=30) args_.last_click_point = None elif event.type == pygame.KEYUP: if event.key == pygame.K_r: # reset rotation args_.spectator.set_transform( carla.Transform(rotation=carla.Rotation(pitch=0.0, yaw=0.0, roll=0.0), location=prev_location)) elif event.key == pygame.K_t: # print out the camera transform print(args_.spectator.get_transform()) print("camera FOV: %s" % \ args_.rgb_camera.camera_actor.attributes["fov"]) # back to preset location elif event.key == pygame.K_y: if args_.preset is not None: args_.spectator.set_transform(args_.preset) # save the image in camera_to_save to save_path elif event.key == pygame.K_p: # in asyn mode, the frame_number might be different for the two # cameras save_rgb_image( args_.seg_camera.rgb_image, os.path.join( save_screenshot_path, "%08d.seg.jpg" % args_.seg_camera.last_image_frame_num)) save_rgb_image( args_.rgb_camera.rgb_image, os.path.join( save_screenshot_path, "%08d.rgb.jpg" % args_.rgb_camera.last_image_frame_num)) print( "Saved seg and rgb image to %s, framenum %s, timestamp %s, " % (save_screenshot_path, args_.rgb_camera.last_image_frame_num, args_.rgb_camera.last_image_seconds)) # toggle saving to videos elif event.key == pygame.K_b: if not args_.rgb_camera.recording: print("Start saving video frames to %s" % args_.rgb_camera.save_path) args_.rgb_camera.recording = True print("\tScene segmentation to %s" % args_.seg_camera.save_path) args_.seg_camera.recording = True else: print("stop saving video frame & seg feature.") args_.rgb_camera.recording = False args_.seg_camera.recording = False elif event.key == pygame.K_x: # before recording the video, remember to hit this button to get all # actor bboxes # update the actors in the scene, like after running build moment # reset all the vehicle and walker actor list # the actual box will be computed in client loop vehicles_[:] = [] walkers_[:] = [] if args_.show_actor_box: args_.show_actor_box = False print("stop showing box.") else: args_.show_actor_box = True for v in world_.get_actors().filter('vehicle.*'): vehicles_.append(v) for w in world.get_actors().filter('walker.*'): walkers_.append(w) print( "Start showing actor boxes: got %s vehicles, %s walkers." % (len(vehicles_), len(walkers_))) # an ugly way to change the camera fov elif (event.key == pygame.K_n) or (event.key == pygame.K_m): if event.key == pygame.K_n: new_fov = args.prev_camera_fov - 5.0 else: new_fov = args.prev_camera_fov + 5.0 new_fov = new_fov if new_fov >= 5.0 else 5.0 new_fov = new_fov if new_fov <= 175.0 else 175.0 prev_recording = args_.rgb_camera.recording args_.camera_bp.set_attribute("fov", "%s" % new_fov) args_.camera_seg_bp.set_attribute("fov", "%s" % new_fov) args_.camera_depth_bp.set_attribute("fov", "%s" % new_fov) # destroy the original actor and make a new camera object args_.rgb_camera.camera_actor.stop() args_.seg_camera.camera_actor.stop() args_.depth_camera.camera_actor.stop() commands_ = [ # destroy the previous actor first carla.command.DestroyActor( args_.depth_camera.camera_actor.id), carla.command.DestroyActor( args_.seg_camera.camera_actor.id), carla.command.DestroyActor( args_.rgb_camera.camera_actor.id), # spawn the new actor carla.command.SpawnActor(args_.camera_bp, carla.Transform(), args_.spectator), carla.command.SpawnActor(args_.camera_seg_bp, carla.Transform(), args_.spectator), carla.command.SpawnActor(args_.camera_depth_bp, carla.Transform(), args_.spectator), ] response_ = client_.apply_batch_sync(commands_) camera_actor_ids_ = [r.actor_id for r in response_[-3:]] camera_, camera_seg_, camera_depth_ = world.get_actors( camera_actor_ids_) args_.rgb_camera = Camera(camera_, width=args_.width, height=args_.height, recording=prev_recording, fov=new_fov, save_path=args_.save_video_path, camera_type="rgb") args_.seg_camera = Camera( camera_seg_, save_path=args_.save_seg_path, camera_type="seg", seg_save_img=args_.save_seg_as_img, recording=prev_recording, image_type=carla.ColorConverter.CityScapesPalette) args_.depth_camera = Camera(camera_depth_, save_path=None, camera_type="depth", recording=prev_recording) args_.prev_camera_fov = new_fov # get a big dict of what key is pressed now, so to avoid hitting forward # multiple times to go forward for a distance step = 0.1 * ms_since_last_tick # this is from experimenting keys = pygame.key.get_pressed() if keys[pygame.K_w]: args_.spectator.set_location(prev_location + step * global_forward_vector) if keys[pygame.K_s]: args_.spectator.set_location(prev_location - step * global_forward_vector) if keys[pygame.K_a]: args_.spectator.set_location(prev_location + step * left_vector) if keys[pygame.K_d]: args_.spectator.set_location(prev_location - step * left_vector) if keys[pygame.K_u]: args_.spectator.set_location(prev_location + step * 0.5 * global_up_vector) if keys[pygame.K_i]: args_.spectator.set_location(prev_location - step * 0.5 * global_up_vector) if keys[pygame.K_UP]: args_.spectator.set_transform( carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch + 1.0, yaw=prev_rotation.yaw, roll=prev_rotation.roll), location=prev_location)) if keys[pygame.K_DOWN]: args_.spectator.set_transform( carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch - 1.0, yaw=prev_rotation.yaw, roll=prev_rotation.roll), location=prev_location)) if keys[pygame.K_LEFT]: args_.spectator.set_transform( carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch, yaw=prev_rotation.yaw - 1.0, roll=prev_rotation.roll), location=prev_location)) if keys[pygame.K_RIGHT]: args_.spectator.set_transform( carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch, yaw=prev_rotation.yaw + 1.0, roll=prev_rotation.roll), location=prev_location)) return False
def point_triangle_distance(points, verts, faces): n_p = points.shape[0] n_f = faces.shape[0] # make sure everything is contiguous points = points.contiguous() verts = verts.contiguous() faces = faces.contiguous() points_expand = points.unsqueeze(1).expand(-1, n_f, -1) face_normals = utils.face_normals(verts, faces) # Accumulate distances dists2 = float('inf') * torch.ones( (n_p, n_f), dtype=points.dtype, device=points.device) # True if point projects inside of face in plane inside_face = torch.ones((n_p, n_f), dtype=torch.bool, device=points.device) for i in range(3): # Distance to each of the three edges lineA = verts[faces[:, i]] lineB = verts[faces[:, (i + 1) % 3]] dists2 = torch.min(dists2, point_line_segment_distances2(points, lineA, lineB)) # Edge perp vec (not normalized) e_perp = utils.cross(face_normals, lineB - lineA) inside_edge = utils.dot( e_perp.unsqueeze(0).expand(n_p, -1, -1), points_expand - lineA.unsqueeze(0).expand(n_p, -1, -1)) > 0 inside_face = inside_face & inside_edge dists = torch.sqrt(dists2) # For points inside, distance is just normal distance point_in_face = verts[faces[:, 0]].unsqueeze(0).expand(n_p, -1, -1) inside_face_dist = torch.abs( utils.dot( face_normals.unsqueeze(0).expand(n_p, -1, -1)[inside_face], points_expand[inside_face] - point_in_face[inside_face])) # dists[inside_face] = inside_face_dist inside_face_dist_full = torch.zeros_like(dists) inside_face_dist_full[inside_face] = inside_face_dist dists = torch.where(inside_face, inside_face_dist_full, dists) if False: polyscope.remove_all_structures() samp = polyscope.register_point_cloud("points", toNP(points)) samp.add_scalar_quantity("dist", toNP(dists[:, 0])) samp.add_scalar_quantity("inside face", toNP(inside_face[:, 0].float())) tri = polyscope.register_surface_mesh("tri", toNP(verts), toNP(faces[0, :].unsqueeze(0))) tri.add_face_vector_quantity("e perp", toNP(e_perp[0, :].unsqueeze(0))) tri.add_face_vector_quantity("e vec", toNP((lineB - lineA)[0, :].unsqueeze(0))) tri.add_face_vector_quantity("N", toNP((face_normals)[0, :].unsqueeze(0))) polyscope.show() return dists
def showEdges(): fig = plt.figure() fig.canvas.mpl_connect('button_press_event', onclick) #calculate smoothed normalmap smoothing = 3 maxdepth = 0 width = utils.getWidth() height = utils.getHeight() normalmap = np.zeros((width, height, 3)) for x in range(1 + smoothing, width - 1 - smoothing): for y in range(1 + smoothing, height - 1 - smoothing): maxdepth = max(maxdepth, utils.parseDepth(x, y)) cmx = utils.convert2Dto3D( calibration[1], x - 1, y, utils.parseDepthSmoothed(x - 1, y, smoothing)) cmy = utils.convert2Dto3D( calibration[1], x, y - 1, utils.parseDepthSmoothed(x, y - 1, smoothing)) cpx = utils.convert2Dto3D( calibration[1], x + 1, y, utils.parseDepthSmoothed(x + 1, y, smoothing)) cpy = utils.convert2Dto3D( calibration[1], x, y + 1, utils.parseDepthSmoothed(x, y + 1, smoothing)) n = utils.normalize( utils.cross(utils.sub(cpx, cmx), utils.sub(cpy, cmy))) normalmap[x][height - y - 1][0] = min(abs(n[0]), 1) normalmap[x][height - y - 1][1] = min(abs(n[1]), 1) normalmap[x][height - y - 1][2] = min(abs(n[2]), 1) #fade normalmap #for x in range(1, width): # for y in range(1, height): # factor = utils.parseDepth(x, y) / maxdepth # normalmap[x][height - y - 1][0] *= (1 - factor) # normalmap[x][height - y - 1][1] *= (1 - factor) # normalmap[x][height - y - 1][2] *= (1 - factor) #calculate edgemap edgemap = np.zeros((width, height, 3)) for x in range(2, width - 2): for y in range(2, height - 2): #calculate edge from depthmap d = utils.convert2Dto3D(calibration[1], x, y, utils.parseDepth(x, y)) mx = utils.convert2Dto3D(calibration[1], x - 1, y, utils.parseDepth(x - 1, y)) my = utils.convert2Dto3D(calibration[1], x, y - 1, utils.parseDepth(x, y - 1)) px = utils.convert2Dto3D(calibration[1], x + 1, y, utils.parseDepth(x + 1, y)) py = utils.convert2Dto3D(calibration[1], x, y + 1, utils.parseDepth(x, y + 1)) edge = max(max(utils.length(d, mx), utils.length(d, my)), max(utils.length(d, px), utils.length(d, py))) if edge > 0.1: # difference of depth in meters edgemap[x][height - y - 1][1] = 1 #calculate edge from normalmap d = normalmap[x][height - y - 1] mx = normalmap[x - 1][height - y - 1] my = normalmap[x][height - y - 1 - 1] px = normalmap[x + 1][height - y - 1] py = normalmap[x][height - y - 1 + 1] edge = max(max(utils.dot(d, mx), utils.dot(d, my)), max(utils.dot(d, px), utils.dot(d, py))) if edge > 0.9: # normal matching in percentage edgemap[x][height - y - 1][0] = 1 #calculate output output = np.zeros((width, height, 3)) for x in range(2, width - 2): for y in range(2, height - 2): if edgemap[x][height - y - 1][1] == 1: output[x][height - y - 1][2] = 1 else: if edgemap[x][height - y - 1][0] == 0: if edgemap[x - 1][height - y - 1][0] == 1: output[x][height - y - 1][1] = 1 if edgemap[x][height - y - 1 - 1][0] == 1: output[x][height - y - 1][1] = 1 if edgemap[x + 1][height - y - 1][0] == 1: output[x][height - y - 1][1] = 1 if edgemap[x][height - y - 1 + 1][0] == 1: output[x][height - y - 1][1] = 1 #plt.imshow(normalmap, extent=[0, height, 0, width]) #plt.imshow(edgemap, extent=[0, height, 0, width]) plt.imshow(output, extent=[0, height, 0, width])
import utils as ut row_units = [ut.cross(r, ut.cols) for r in ut.rows] column_units = [ut.cross(ut.rows, c) for c in ut.cols] square_units = [ut.cross(rs, cs) for rs in ('ABC', 'DEF', 'GHI') for cs in ('123', '456', '789')] unitlist = row_units + column_units + square_units # Update the unit list to add the new diagonal units unitlist = unitlist + [[ut.rows[i] + ut.cols[i] for i in range(9)]] + [[ut.rows[::-1][i] + ut.cols[i] for i in range(9)]] # Must be called after all units (including diagonals) are added to the unitlist units = ut.extract_units(unitlist, ut.boxes) peers = ut.extract_peers(units, ut.boxes) def naked_twins(values): """Eliminate values using the naked twins strategy. The naked twins strategy says that if you have two or more unallocated boxes in a unit and there are only two digits that can go in those two boxes, then those two digits can be eliminated from the possible assignments of all other boxes in the same unit. Parameters ---------- values(dict) a dictionary of the form {'box_name': '123456789', ...} Returns ------- dict
from utils import rows, cols, cross, extract_units, extract_peers, \ boxes, grid2values, display, history, assign_value row_units = [cross(r, cols) for r in rows] column_units = [cross(rows, c) for c in cols] square_units = [ cross(rs, cs) for rs in ('ABC', 'DEF', 'GHI') for cs in ('123', '456', '789') ] unitlist = row_units + column_units + square_units diagonal_1 = [x[0] + x[1] for x in zip(rows, cols)] diagonal_2 = [x[0] + x[1] for x in zip(rows, cols[::-1])] diagonal_units = [diagonal_1] + [diagonal_2] unitlist = unitlist + diagonal_units # Must be called after all units (including diagonals) are added to the unitlist units = extract_units(unitlist, boxes) peers = extract_peers(units, boxes) def naked_twins(values): """Eliminate values using the naked twins strategy. The naked twins strategy says that if you have two or more unallocated boxes in a unit and there are only two digits that can go in those two boxes, then those two digits can be eliminated from the possible assignments of all other boxes in the same unit. Parameters ----------
def calc_acc(AA, ww, q, qd, Ydd, Conn, Prop): """ Returns the linear acceleration of all body centroids and the angular acceleration of all body (eqs. 3.10-3.11 and 3.14-3.15, figure 3.5). vd = [vd_0, vd_1, ... ] (3, num_b) wd = [wd_0, wd_1, ... ] (3, num_b) """ BB, j_type = Conn[2], Conn[3] cc = Prop[2] vd0, wd0, qdd = Ydd[0:3], Ydd[3:6], Ydd[6:] num_b = len(q) + 1 # Number of bodies vd = np.zeros((3, num_b)) wd = np.zeros((3, num_b)) # Base vd[:, 0] = vd0 wd[:, 0] = wd0 # i = current link, k = lower link connection for i in range(1, num_b): idxi = i - 1 # Index link/joint <i> in BB, j_type, q, qd, qdd k = BB[idxi] # Index lower link connection # Rotation matrices A_I_i = AA[:, 3 * i:3 * (i + 1)] A_I_k = AA[:, 3 * k:3 * (k + 1)] # Relative positions cc_I_i = A_I_i @ cc[:, i, i] cc_I_k = A_I_k @ cc[:, k, i] # Joint values q_I_i = A_I_i @ (Ez * q[idxi]) qd_I_i = A_I_i @ (Ez * qd[idxi]) qdd_I_i = A_I_i @ (Ez * qdd[idxi]) # Rotational joint (eqs. 3.10-3.11) if (j_type[idxi] == 'R'): wd[:, i] = wd[:, k] + cross(ww[:, i], qd_I_i) + qdd_I_i vd[:, i] = vd[:, k] + cross(wd[:, k], cc_I_k) \ + cross(ww[:, k], cross(ww[:, k], cc_I_k)) \ - cross(wd[:, i], cc_I_i) \ - cross(ww[:, i], cross(ww[:, i], cc_I_i)) # Prismatic joint (eqs. 3.14-3.15) elif (j_type[idxi] == 'P'): wd[:, i] = wd[:, k] vd[:, i] = vd[:, k] + cross(wd[:, k], cc_I_k) \ + cross(ww[:, k], cross(ww[:, k], cc_I_k)) \ - cross(wd[:, i], cc_I_i) \ - cross(ww[:, i], cross(ww[:, i], cc_I_i)) \ + cross(wd[:, i], q_I_i) \ + cross(ww[:, i], cross(ww[:, i], q_I_i)) \ + 2.0 * cross(ww[:, i], qd_I_i) \ + qdd_I_i return vd, wd
def _cross(cls, v1, v2): return utils.cross(v1, v2)
def r_ne(RR, AA, q, Yd, Ydd, Fe, Te, Conn, Prop): """ Inverse dynamics computation by the recursive Newton-Euler method (eqs. 3.30-3.39). Given: state RR, AA, v0, w0, q, qd accelerations vd0, wd0, qdd external forces and moments Fe, Te Returns: reaction forces and moments F0, T0, tau i.e. the system reaction force <F0> and moment <T0> on the base, and the torque/forces <tau> on the joints. """ SS, SE, j_type = Conn[0], Conn[1], Conn[3] mass, inertia, cc, ce, Qe, gravity = \ Prop[0], Prop[1], Prop[2], Prop[3], Prop[5], Prop[6] qd = Yd[6:] num_j = len(q) # Number of joints/links num_b = num_j + 1 # Number of bodies num_e = SE.shape[1] # Number of endpoints # Linear and angular velocities of all bodies vv, ww = calc_vel(AA, q, Yd, Conn, Prop) # Linear and angular accelerations of all bodies vd, wd = calc_acc(AA, ww, q, qd, Ydd, Conn, Prop) # Inertial forces and moments on the body centroids (for convenience the # the gravitational force is also included here) - eqs. 3.30-3.31 F_in = np.zeros((3, num_b)) T_in = np.zeros((3, num_b)) for i in range(num_b): A_I_i = AA[:, 3*i:3*(i+1)] In_I_i = A_I_i @ inertia[:, 3*i:3*(i+1)] @ A_I_i.T # Eq. 3.30 and 3.31 F_in[:, i] = mass[i] * (vd[:, i] - gravity) T_in[:, i] = In_I_i @ wd[:, i] + cross(ww[:, i], (In_I_i @ ww[:, i])) # Forces and moments on the joints (eqs. 3.32-3.35) F_jnt = np.zeros((3, num_j)) T_jnt = np.zeros((3, num_j)) # Start from the last link for i in range(num_j, 0, -1): idxi = i - 1 # Index joint <i> in Fjnt, Tjnt, j_type, q A_I_i = AA[:, 3*i:3*(i+1)] is_P = float(j_type[idxi] == 'P') # = 1 if joint is prismatic # Vector centroid <i> to joint <i> L_ii = cc[:, i, i] - is_P * Ez * q[idxi] # Add inertial force and moment on the link centroid (the cross-product # is negative because the arm should go from the joint to the centroid) F_jnt[:, idxi] = F_in[:, i] T_jnt[:, idxi] = T_in[:, i] - cross(A_I_i @ L_ii, F_in[:, i]) # Add force and moment due to connected upper links (note that a link # may have more than one upper connection) for j in range(i+1, num_j+1): idxj = j - 1 # Index joint <j> in j_type, q, F_jnt, T_jnt # Add contribution if link <j> is an upper connection of link <i> if (SS[i, j]): # Vector joint <i> to joint <j> L_ij = cc[:, i, j] - cc[:, i, i] + is_P * Ez * q[idxi] # Add joint force and moment (note that Fj and Tj are calculated # as joint force and moment on the j-th link, thus the force and # moment passed to the i-th link are equal and opposite) F_jnt[:, idxi] += F_jnt[:, idxj] T_jnt[:, idxi] += cross(A_I_i @ L_ij, F_jnt[:, idxj]) \ + T_jnt[:, idxj] # Add external forces and moments for ie in range(num_e): # Add contribution if endpoint <ie> is connected to link <i> if (SE[0, ie] == i): # Vector centroid <i> to endpoint <ie> A_i_ie = rpy2dc(Qe[:, ie]).T # Endpoint to link/joint A_I_ie = A_I_i @ A_i_ie # Endpoint to inertial # If the external load is given wrt the local frame if (SE[1, ie] == 0): L_i_ie = A_i_ie.T @ (ce[:, ie] - cc[:, i, i] + is_P * Ez * q[idxi]) F_jnt[:, idxi] -= A_I_ie @ Fe[:, ie] T_jnt[:, idxi] -= A_I_ie @ (tilde(L_i_ie) @ Fe[:, ie] + Te[:, ie]) # If the external load is given wrt the inertial frame else: L_i_ie = A_I_ie @ (ce[:, ie] - cc[:, i, i] + is_P * Ez * q[idxi]) F_jnt[:, idxi] -= Fe[:, ie] T_jnt[:, idxi] -= (tilde(L_i_ie) @ Fe[:, ie] + Te[:, ie]) # Reaction force and moment on the base centroid (Eqs. 3.38 and 3.39) F0 = np.zeros(3) T0 = np.zeros(3) # Add inertial force and moment of the base F0 += F_in[:, 0] T0 += T_in[:, 0] # Add forces and moments from the links connected to the base for i in range(1, num_j+1): # Add if link <i> is connected if (SS[0, i] > 0): idxi = i - 1 # Index link/joint <i> in Fjnt, Tjnt F0 += F_jnt[:, idxi] T0 += cross(AA[:, 0:3] @ cc[:, 0, i], F_jnt[:, idxi]) \ + T_jnt[:, idxi] # Add external forces and moments for ie in range(num_e): # Add contribution if endpoint <ie> is connected to the base if (SE[0, ie] == 0): A_0_ie = rpy2dc(Qe[:, ie]).T # Endpoint to base A_I_ie = AA[:, 0:3] @ A_0_ie # Endpoint to inertial # If the external load is given wrt the local frame if (SE[1, ie] == 0): R_0_ie = A_0_ie.T @ ce[:, ie] F0 -= A_I_ie @ Fe[:, ie] T0 -= A_I_ie @ (tilde(R_0_ie) @ Fe[:, ie] + Te[:, ie]) # If the external load is given wrt the inertial frame else: R_0_ie = A_0_ie @ ce[:, ie] F0 -= Fe[:, ie] T0 -= (tilde(R_0_ie) @ Fe[:, ie] + Te[:, ie]) # Calculation of joint torques/forces (eq. 3.36 and 3.37) tau = np.zeros(num_j) for i in range(1, num_j+1): idxi = i - 1 # Index link/joint <i> in Fjnt, Tjnt, j_type, tau Ez_I_i = AA[:, 3*i:3*(i+1)] @ Ez # If revolute joint (eq. 3.36) if (j_type[idxi] == 'R'): tau[idxi] = T_jnt[:, idxi] @ Ez_I_i # If prismatic joint (eq. 3.37) elif (j_type[idxi] == 'P'): tau[idxi] = F_jnt[:, idxi] @ Ez_I_i # Compose generalized forces Force = np.block([F0, T0, tau]) return Force
def testFunc(**kwargs): print(kwargs) # prints the dictionary of keyword arguments mlp = alg( **kwargs ) scores = cross_val_score(mlp, X, Y, cv=folds) print(scores) print("Accuracy: %0.2f (+/- %0.2f)" % (scores.mean(), scores.std() * 2)) sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../core") sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../configs") import utils #utils.cross(params_collection, testFunc) import data import output X,Y = data.get_full_data() import mlp configurations = mlp.getconfig() folds = configurations['folds'] configs = configurations['configurations'] for config in configs: alg = config['algorithm_name'] utils.cross(config['parameters'], testFunc)
if i % 100 == 0: print('Epoch {}: {:.4f}'.format(i, loss)) AE.eval() with torch.no_grad(): data_transformed = AE.encode(data_torch).detach().numpy() else: raise Exception('Please specify a valid solver!') edges2d = [] for e in edge_list: edges2d.append(data_transformed[[e[0], e[1]]]) # check crosses crosses = cross(edges2d, edge_list, findall=True) print('{}: {} crosses'.format(method, len(crosses))) plt.scatter(data_transformed[:, 0], data_transformed[:, 1], c=t, cmap=plt.cm.Spectral) lc = LineCollection(segments=edges2d, linewidths=0.3) plt.gca().add_collection(lc) plt.xticks([], []) plt.yticks([], []) if save_fig: filels = glob.glob(fig_dir + '*{}*.pdf'.format(method)) if len(filels) != 0: for file in filels: