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 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()
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()
