def BuildConfigurationFromFile(self, filename): # Delete the exiting configuration first self.DeleteConfiguration() # Build new configurations self.tree = ET.parse(filename) root = self.tree.getroot() modules = root.find("modules") for eachmodule in modules.findall('module') : model_name = eachmodule.find('name').text positionstr = eachmodule.find('position').text # print "position: ",self.StringToTuple(positionstr) module_position = self.StringToTuple(positionstr) jointanglestr = eachmodule.find('joints').text try: module_path = eachmodule.find('path').text except Exception, e: module_path = self.DEFAULT_PATH print "module path: ",module_path # print "joint angles: ",self.StringToTuple(jointanglestr) module_jointangle = self.StringToTuple(jointanglestr) if len(module_position) == 6: new_module = Module(model_name,module_position,module_jointangle,module_path) new_module.rotation_matrix = kinematics.rotz(module_position[5])* \ kinematics.roty(module_position[4])* \ kinematics.rotx(module_position[3]) elif len(module_position) == 7: new_module = Module(model_name,module_position,module_jointangle,module_path,True) new_module.rotation_matrix = kinematics.quatToRot(module_position[3:]) self.ModuleList.append(new_module) if self.ServerConnected == 1: self.PublishMessage(self.ModuleList[-1])
def InsertModel(self,*args): print "frob called with {} arguments".format(len(args)) insertedModel = False if self.InsertMethod.get() == "Position" : print "Method is position" module_position = (self.X_coor.get(),self.Y_coor.get(),self.Z_coor.get(),degree2rad(self.row.get()),degree2rad(self.pitch.get()),degree2rad(self.yaw.get())) print "Position tuple is ",module_position module_jointangle = (degree2rad(self.Joint0.get()),degree2rad(self.Joint1.get()),degree2rad(self.Joint2.get()),degree2rad(self.Joint3.get())) print "Joint angle tuple is ",module_jointangle new_module = Module(self.modelname.get(),module_position,module_jointangle,self.current_modeule_path) # Assign rotation matrix to new_module based on euler angles: new_module.rotation_matrix = kinematics.rotz(module_position[5])* \ kinematics.roty(module_position[4])* \ kinematics.rotx(module_position[3]) self.ModuleList.append(new_module) if self.ServerConnected == 1: self.PublishMessage(self.ModuleList[-1]) insertedModel = True else: print "Method is Connection" theOtherModule = self.findModule(self.connectedmodelvar.get()) if theOtherModule: module_jointangle= (degree2rad(self.Joint0.get()),degree2rad(self.Joint1.get()),degree2rad(self.Joint2.get()),degree2rad(self.Joint3.get())) print "Configuration generator joint angles: ",module_jointangle #-- Get module position and orientation. parent_face = self.Node2.get() new_module_face = self.Node1.get() (module_position, rotation_matrix, quaternion) = kinematics.get_new_position(theOtherModule, module_jointangle, parent_face, new_module_face) print 'XYZ: ' + str(module_position[0:3]) print 'RPY: ' + str(module_position[3:6]) print 'Quat: ' + str(quaternion) # -- new_module = Module(self.modelname.get(),tuple(list(module_position[0:3])+list(quaternion)),module_jointangle, self.current_modeule_path,True) # Add rotation matrix to new_module (necessary for kinematics) new_module.rotation_matrix = rotation_matrix self.ModuleList.append(new_module) print "Connected module name",self.connectedmodelvar.get() new_connection = Connection(self.modelname.get(),self.connectedmodelvar.get(),self.Node1.get(),self.Node2.get(),self.C_dis.get(),self.aoffset.get()) self.ConnectionList.append(new_connection) self.ModuleList[-1].connection(self.Node1.get(),self.ConnectionList[-1]) theOtherModule.connection(self.Node2.get(),self.ConnectionList[-1]) if self.ServerConnected == 1: self.PublishMessage(self.ModuleList[-1]) insertedModel = True if insertedModel: self.updateModuleList() self.nameIncrement() self.Rel_pos.select() self.DisableXYZInput() self.SaveEnable() print "Combox value", self.connectmodel.get() if self.connectmodel.get() != '': self.checkConnectivity()