def iterate(self,context): (A, B, self.d_stats) = make_pairs(self.align_obj, self.base_obj, self.base_bvh, self.vlist, self.thresh, self.sample_factor, calc_stats = self.use_target) if self.align_meth == '0': #rigid transform M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True) elif self.align_meth == '1': # rot, loc, scale M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True) new_mat = Matrix.Identity(4) for y in range(0,4): for z in range(0,4): new_mat[y][z] = M[y][z] self.align_obj.matrix_world = self.align_obj.matrix_world * new_mat trans = new_mat.to_translation() quat = new_mat.to_quaternion() self.align_obj.update_tag() if self.d_stats: i = fmod(self.total_iters,5) self.conv_t_list[i] = trans.length self.conv_r_list[i] = abs(quat.angle) if all(d < self.target_d for d in self.conv_t_list): self.converged = True print('Converged in %s iterations' % str(self.total_iters+1)) print('Final Translation: %f ' % self.conv_t_list[i]) print('Final Avg Dist: %f' % self.d_stats[0]) print('Final St Dev %f' % self.d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(self.conv_r_list))
def moment_arm(F, th, joint_name): if joint_name=='KFE': a = 0.0826 b = 0.1143 c = 0.0818 d = 0.0825 alpha = 4.468 - th elif joint_name == 'HFE': a = 0.0826 b = 0.1143 c = 0.0512 d = 0.0497 # alpha = 3.189 - th alpha = 4.2369 - th elif joint_name == 'HAA': a = 0 b = 0 c = 0.0419 d = 0.1901 alpha = 1.7809 + th e = sqrt(c**2 + d**2 - 2*c*d*cos( alpha)) beta = arccos( (e**2 + d**2 - c**2)/(2*e*d) ) if a != 0: beta2 = arccos( (e**2 + a**2 - b**2)/(2*e*a) ) if(fmod(alpha, 2*pi) > pi): beta = beta2 - beta else: beta = beta2 + beta tau = F * d * sin(beta) return tau
def execute(self, context): settings = get_addon_preferences() align_meth = settings.align_meth start = time.time() align_obj = context.object base_obj = [ obj for obj in context.selected_objects if obj != align_obj ][0] base_bvh = BVHTree.FromObject(base_obj, context.scene) align_obj.rotation_mode = 'QUATERNION' vlist = [] #figure out if we need to do any inclusion/exclusion group_lookup = {g.name: g.index for g in align_obj.vertex_groups} if 'icp_include' in align_obj.vertex_groups: group = group_lookup['icp_include'] for v in align_obj.data.vertices: for g in v.groups: if g.group == group and g.weight > 0.9: vlist.append(v.index) elif 'icp_exclude' in align_obj.vertex_groups: group = group_lookup['icp_exclude'] for v in align_obj.data.vertices: v_groups = [g.group for g in v.groups] if group not in v_groups: vlist.append(v.index) else: for g in v.groups: if g.group == group and g.weight < 0.1: vlist.append(v.index) #unfortunate way to do this.. else: vlist = [v.index for v in align_obj.data.vertices] settings = get_addon_preferences() thresh = settings.min_start sample = settings.sample_fraction iters = settings.icp_iterations target_d = settings.target_d use_target = settings.use_target factor = round(1 / sample) n = 0 converged = False conv_t_list = [target_d * 2] * 5 #store last 5 translations conv_r_list = [None] * 5 while n < iters and not converged: (A, B, d_stats) = make_pairs(align_obj, base_obj, base_bvh, vlist, thresh, factor, calc_stats=use_target) if align_meth == '0': #rigid transform M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True) elif align_meth == '1': # rot, loc, scale M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True) new_mat = Matrix.Identity(4) for y in range(0, 4): for z in range(0, 4): new_mat[y][z] = M[y][z] align_obj.matrix_world = align_obj.matrix_world * new_mat trans = new_mat.to_translation() quat = new_mat.to_quaternion() align_obj.update_tag() context.scene.update() if d_stats: i = fmod(n, 5) conv_t_list[i] = trans.length conv_r_list[i] = abs(quat.angle) if all(d < target_d for d in conv_t_list): converged = True print('Converged in %s iterations' % str(n + 1)) print('Final Translation: %f ' % conv_t_list[i]) print('Final Avg Dist: %f' % d_stats[0]) print('Final St Dev %f' % d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list)) n += 1 time_taken = time.time() - start if use_target and not converged: print('Maxed out iterations') print('Final Translation: %f ' % conv_t_list[i]) print('Final Avg Dist: %f' % d_stats[0]) print('Final St Dev %f' % d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list)) print('Aligned obj in %f sec' % time_taken) return {'FINISHED'}
def execute(self, context): settings = get_settings() align_meth = settings.align_meth start = time.time() align_obj = context.object base_obj = [obj for obj in context.selected_objects if obj != align_obj][0] base_bvh = BVHTree.FromObject(base_obj, context.scene) align_obj.rotation_mode = 'QUATERNION' vlist = [] #figure out if we need to do any inclusion/exclusion group_lookup = {g.name: g.index for g in align_obj.vertex_groups} if 'icp_include' in align_obj.vertex_groups: group = group_lookup['icp_include'] for v in align_obj.data.vertices: for g in v.groups: if g.group == group and g.weight > 0.9: vlist.append(v.index) elif 'icp_exclude' in align_obj.vertex_groups: group = group_lookup['icp_exclude'] for v in align_obj.data.vertices: v_groups = [g.group for g in v.groups] if group not in v_groups: vlist.append(v.index) else: for g in v.groups: if g.group == group and g.weight < 0.1: vlist.append(v.index) #unfortunate way to do this.. else: vlist = [v.index for v in align_obj.data.vertices] settings = get_settings() thresh = settings.min_start sample = settings.sample_fraction iters = settings.icp_iterations target_d = settings.target_d use_target = settings.use_target factor = round(1/sample) n = 0 converged = False conv_t_list = [target_d * 2] * 5 #store last 5 translations conv_r_list = [None] * 5 while n < iters and not converged: (A, B, d_stats) = make_pairs(align_obj, base_obj, base_bvh, vlist, thresh, factor, calc_stats = use_target) if align_meth == '0': #rigid transform M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True) elif align_meth == '1': # rot, loc, scale M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True) new_mat = Matrix.Identity(4) for y in range(0,4): for z in range(0,4): new_mat[y][z] = M[y][z] align_obj.matrix_world = align_obj.matrix_world * new_mat trans = new_mat.to_translation() quat = new_mat.to_quaternion() align_obj.update_tag() context.scene.update() if d_stats: i = fmod(n,5) conv_t_list[i] = trans.length conv_r_list[i] = abs(quat.angle) if all(d < target_d for d in conv_t_list): converged = True print('Converged in %s iterations' % str(n+1)) print('Final Translation: %f ' % conv_t_list[i]) print('Final Avg Dist: %f' % d_stats[0]) print('Final St Dev %f' % d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list)) n += 1 time_taken = time.time() - start if use_target and not converged: print('Maxed out iterations') print('Final Translation: %f ' % conv_t_list[i]) print('Final Avg Dist: %f' % d_stats[0]) print('Final St Dev %f' % d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list)) print('Aligned obj in %f sec' % time_taken) return {'FINISHED'}
def execute(self, context): settings = get_settings() ICP_sel = context.scene.ICP_area align_meth = settings.align_meth start = time.time() align_obj = context.object base_obj = [ obj for obj in context.selected_objects if obj != align_obj ][0] # **********************choose select vertexgroup in baseobj to ICP*********************** # find polygons from vetex group # get list of vertex in selected vertex groups sel_poly = [] base_vlist = [] base_vlist_co = [] base_group_lookup = {g.name: g.index for g in base_obj.vertex_groups } #create a dictionary base_group_vlist = [] if ICP_sel in base_obj.vertex_groups: group = base_group_lookup[ICP_sel] for v in base_obj.data.vertices: for g in v.groups: if g.group == group: base_group_vlist.append(v.index) #get list of polygon from selected vertex group b = set(base_group_vlist) for poly in base_obj.data.polygons: a = set(poly.vertices[:]) anb = a & b if anb: verts_in_poly = poly.vertices[:] sel_poly.append(poly.index) #selected polygons base_vlist.append( poly.vertices[:]) #selected vertex from polygons #renumber polygons vertices # combine all vertice in polygons list sortlist = [] for poly in base_vlist: for vert in poly: sortlist.append(vert) sortlist = sorted( set(sortlist)) # remove duplicate and sort the list # renumber base_vlist to index of found item in sortlist # for poly in base_vlist: # for i, vert in enumerate(poly): # for j, no in enumerate(sortlist): # if vert==no: # # vert = j # poly[i] = j --> error: 'tuple' object does not support item assignment temp = [[vert for vert in poly] for poly in base_vlist ] #need to copy to another list to be able to modified for poly in temp: for i, vert in enumerate(poly): for j, no in enumerate(sortlist): if vert == no: poly[i] = j # Create list of vertex coordination from softlist for vert in sortlist: base_vlist_co.append( base_obj.data.vertices[vert].co ) #coord of selected vertex in selected polygons base_bvh = BVHTree.FromPolygons( base_vlist_co, temp) #choose selected vertices in baseobj else: base_bvh = BVHTree.FromObject( base_obj, context.scene) #choose whole vertices in baseobj # **********************choose select vertexgroup in alignobj to ICP*********************** align_obj.rotation_mode = 'QUATERNION' vlist = [] group_lookup = {g.name: g.index for g in align_obj.vertex_groups} if ICP_sel in align_obj.vertex_groups: group = group_lookup[ICP_sel] for v in align_obj.data.vertices: #copy selected area vertex to list of ICP action for g in v.groups: if g.group == group: vlist.append(v.index) else: #choose the whole align objects vertex if not choose the area vlist = [v.index for v in align_obj.data.vertices] # ************************************************************************************ settings = get_settings() thresh = settings.min_start sample = settings.sample_fraction iters = settings.icp_iterations target_d = settings.target_d use_target = settings.use_target factor = round(1 / sample) n = 0 converged = False conv_t_list = [target_d * 2] * 5 #store last 5 translations conv_r_list = [None] * 5 bematrixworld = align_obj.matrix_world.copy( ) #store the begining matrixworld while n < iters and not converged: (A, B, d_stats) = make_pairs(align_obj, base_obj, base_bvh, vlist, thresh, factor, calc_stats=use_target) if align_meth == '0': #rigid transform M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True) # print (M) elif align_meth == '1': # rot, loc, scale M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True) new_mat = Matrix.Identity(4) for y in range(0, 4): for z in range(0, 4): new_mat[y][z] = M[y][z] #new_mat = affine transform matrix align_obj.matrix_world = align_obj.matrix_world * new_mat #****This is for transformation every interations trans = new_mat.to_translation() quat = new_mat.to_quaternion() align_obj.update_tag() context.scene.update() if d_stats: i = fmod(n, 5) #returns the floating-point remainder of x/y conv_t_list[ i] = trans.length #get the length of last 5 translation matrixs conv_r_list[i] = abs(quat.angle) if all(d < target_d for d in conv_t_list): converged = True print('---------Summary-----------------') print('Converged in %s iterations' % str(n + 1)) print('Final Translation: %f ' % conv_t_list[i]) print('Final Avg Dist: %f' % d_stats[0]) print('Final St Dev %f' % d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list)) n += 1 time_taken = time.time() - start if use_target and not converged: print('Maxed out iterations') print('Final Translation: %f ' % conv_t_list[i]) print('Final Avg Dist: %f' % d_stats[0]) print('Final St Dev %f' % d_stats[1]) print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list)) print('Aligned obj in %f sec' % time_taken) endmatrixworld = align_obj.matrix_world #store the last matrixworld ICPmatrix = endmatrixworld * bematrixworld.inverted() Initial_point = Vector([1.78237, 0.69558, 2.40553]) point_transformed = ICPmatrix * Initial_point accessfile = bpy.path.abspath("//output.txt") with open(accessfile, 'a') as f: f.write('The begining world maxtrix:\n') f.write(str(bematrixworld) + '\n') f.write('The ended world matrix:\n') f.write(str(endmatrixworld) + '\n') f.write('The ICP transformation matrix is:\n') f.write(str(ICPmatrix) + '\n') f.write('The inverted ICP transformation matrix is:\n') f.write(str(ICPmatrix.inverted()) + '\n') f.write('lefteye vertex coord:\n') f.write(str(Initial_point) + '\n') f.write("transform to:\n") f.write(str(point_transformed) + '\n') f.write( '---------------------*************------------------------\n') return {'FINISHED'}