def solveSystem(self,doc,matelist=None):
if not a2plib.SIMULATION_STATE:
Msg( "\n===== Start Solving System ====== \n" )
systemSolved = self.solveAccuracySteps(doc,matelist)
if self.status == "loadingDependencyError":
return systemSolved
if systemSolved:
self.status = "solved"
if not a2plib.SIMULATION_STATE:
Msg( "===== System solved using partial + recursive unfixing =====\n")
self.checkForUnmovedParts()
else:
if a2plib.SIMULATION_STATE == True:
self.status = "unsolved"
return systemSolved
else: # a2plib.SIMULATION_STATE == False
self.status = "unsolved"
Msg( "===== Could not solve system ====== \n" )
msg = \
'''
Constraints inconsistent. Cannot solve System.
Please delete your last created constraint !
'''
QtGui.QMessageBox.information(
QtGui.QApplication.activeWindow(),
"Constraint mismatch",
msg
)
return systemSolved
def solveSystem(self, doc, matelist=None, showFailMessage=True):
if not a2plib.SIMULATION_STATE:
Msg("\n===== Start Solving System ====== \n")
systemSolved = self.solveAccuracySteps(doc, matelist)
if self.status == "loadingDependencyError":
return systemSolved
if systemSolved:
self.status = "solved"
if not a2plib.SIMULATION_STATE:
Msg("===== System solved using partial + recursive unfixing =====\n"
)
self.checkForUnmovedParts()
else:
if a2plib.SIMULATION_STATE == True:
self.status = "unsolved"
return systemSolved
else: # a2plib.SIMULATION_STATE == False
self.status = "unsolved"
if showFailMessage == True:
Msg("===== Could not solve system ====== \n")
msg = \
translate("A2plus_solversystem",
'''
Constraints inconsistent. Cannot solve System.
Please run the conflict finder tool !
'''
)
QtGui.QMessageBox.information(
QtGui.QApplication.activeWindow(),
translate("A2plus_solversystem",
"Constraint mismatch"), msg)
return systemSolved
def solveAccuracySteps(self, doc, matelist=None):
self.level_of_accuracy = 1
self.mySOLVER_POS_ACCURACY = self.getSolverControlData()[
self.level_of_accuracy][0]
self.mySOLVER_SPIN_ACCURACY = self.getSolverControlData()[
self.level_of_accuracy][1]
self.loadSystem(doc, matelist)
if self.status == "loadingDependencyError":
return
self.assignParentship(doc)
while True:
systemSolved = self.calculateChain(doc)
if self.level_of_accuracy == 1:
self.detectUnmovedParts(
) # do only once here. It can fail at higher accuracy levels
# where not a final solution is required.
if a2plib.SOLVER_ONESTEP > 0:
systemSolved = True
break
if systemSolved:
self.level_of_accuracy += 1
if self.level_of_accuracy > len(self.getSolverControlData()):
self.solutionToParts(doc)
break
self.mySOLVER_POS_ACCURACY = self.getSolverControlData()[
self.level_of_accuracy][0]
self.mySOLVER_SPIN_ACCURACY = self.getSolverControlData()[
self.level_of_accuracy][1]
self.loadSystem(doc, matelist)
else:
completeSolvingRequired = self.getSolverControlData()[
self.level_of_accuracy][2]
if not completeSolvingRequired: systemSolved = True
break
self.maxAxisError = 0.0
self.maxSingleAxisError = 0.0
self.maxPosError = 0.0
for rig in self.rigids:
if rig.maxPosError > self.maxPosError:
self.maxPosError = rig.maxPosError
if rig.maxAxisError > self.maxAxisError:
self.maxAxisError = rig.maxAxisError
if rig.maxSingleAxisError > self.maxSingleAxisError:
self.maxSingleAxisError = rig.maxSingleAxisError
if not a2plib.SIMULATION_STATE:
Msg('TARGET POS-ACCURACY :{}\n'.format(
self.mySOLVER_POS_ACCURACY))
Msg('REACHED POS-ACCURACY :{}\n'.format(self.maxPosError))
Msg('TARGET SPIN-ACCURACY :{}\n'.format(
self.mySOLVER_SPIN_ACCURACY))
Msg('REACHED SPIN-ACCURACY :{}\n'.format(self.maxAxisError))
Msg('SA SPIN-ACCURACY :{}\n'.format(self.maxSingleAxisError))
return systemSolved
def solveAccuracySteps(self, doc):
self.level_of_accuracy = 1
self.mySOLVER_POS_ACCURACY = SOLVER_CONTROLDATA[
self.level_of_accuracy][0]
self.mySOLVER_SPIN_ACCURACY = SOLVER_CONTROLDATA[
self.level_of_accuracy][1]
#startTime = int(round(time.time() * 1000))
self.loadSystem(doc)
if self.status == "loadingDependencyError":
return
self.assignParentship(doc)
#loadTime = int(round(time.time() * 1000))
while True:
systemSolved = self.calculateChain(doc)
#totalTime = int(round(time.time() * 1000))
if systemSolved:
self.level_of_accuracy += 1
if self.level_of_accuracy > len(SOLVER_CONTROLDATA):
self.solutionToParts(doc)
break
#self.prepareRestart()
self.mySOLVER_POS_ACCURACY = SOLVER_CONTROLDATA[
self.level_of_accuracy][0]
self.mySOLVER_SPIN_ACCURACY = SOLVER_CONTROLDATA[
self.level_of_accuracy][1]
#self.solutionToParts(doc)
self.loadSystem(doc)
else:
#self.solutionToParts(doc)
completeSolvingRequired = SOLVER_CONTROLDATA[
self.level_of_accuracy][2]
if not completeSolvingRequired: systemSolved = True
break
self.maxAxisError = 0.0
self.maxPosError = 0.0
for rig in self.rigids:
if rig.maxPosError > self.maxPosError:
self.maxPosError = rig.maxPosError
if rig.maxAxisError > self.maxAxisError:
self.maxAxisError = rig.maxAxisError
Msg('TARGET POS-ACCURACY :{}\n'.format(self.mySOLVER_POS_ACCURACY))
Msg('REACHED POS-ACCURACY :{}\n'.format(self.maxPosError))
Msg('TARGET SPIN-ACCURACY :{}\n'.format(self.mySOLVER_SPIN_ACCURACY))
Msg('REACHED SPIN-ACCURACY :{}\n'.format(self.maxAxisError))
return systemSolved
def assignParentship(self, doc):
# Start from fixed parts
for rig in self.rigids:
if rig.fixed:
rig.disatanceFromFixed = 0
haveMore = True
distance = 0
while haveMore:
haveMore = rig.assignParentship(distance)
distance += 1
if A2P_DEBUG_LEVEL > 0:
Msg(20 * "=" + "\n")
Msg("Hierarchy:\n")
Msg(20 * "=" + "\n")
for rig in self.rigids:
if rig.fixed: rig.printHierarchy(0)
Msg(20 * "=" + "\n")
def visualizeHierarchy(self):
'''
generate a html file with constraints structure.
The html file is in the same folder
with the same filename of the assembly
'''
out_file = os.path.splitext(self.doc.FileName)[0] + '_asm_hierarchy.html'
Msg("Writing visual hierarchy to: {}\n".format(out_file))
f = open(out_file, "w")
f.write("<!DOCTYPE html>\n")
f.write("<html>\n")
f.write("<head>\n")
f.write(' <meta charset="utf-8">\n')
f.write(' <meta http-equiv="X-UA-Compatible" content="IE=edge">\n')
f.write(' <title>A2P assembly hierarchy visualization</title>\n')
f.write("</head>\n")
f.write("<body>\n")
f.write('<div class="mermaid">\n')
f.write("graph TD\n")
for rig in self.rigids:
rigLabel = a2plib.to_str(rig.label).replace(u' ',u'_')
# No children, add current rogod as a leaf entry
if len(rig.childRigids) == 0:
message = u"{}\n".format(rigLabel)
if a2plib.PYVERSION < 3:
f.write(a2plib.to_bytes(message))
else:
f.write(message)
else:
# Rigid have children, add them based on the dependency list
for d in rig.dependencies:
if d.dependedRigid in rig.childRigids:
dependedRigLabel = a2plib.to_str(d.dependedRigid.label).replace(u' ',u'_')
if rig.fixed:
message = u"{}({}<br>*FIXED*) -- {} --> {}\n".format(rigLabel, rigLabel, d.Type, dependedRigLabel)
if a2plib.PYVERSION < 3:
f.write(a2plib.to_bytes(message))
else:
f.write(message)
else:
message = u"{} -- {} --> {}\n".format(rigLabel, d.Type, dependedRigLabel)
if a2plib.PYVERSION < 3:
f.write(a2plib.to_bytes(message))
else:
f.write(message)
f.write("</div>\n")
f.write(' <script src="https://unpkg.com/[email protected]/dist/mermaid.js"></script>\n')
f.write(" <script>\n")
f.write(' mermaid.initialize({startOnLoad: true});\n')
f.write(" </script>\n")
f.write("</body>")
f.write("</html>")
f.close()
def DOF_info_to_console(self):
self.loadSystem(FreeCAD.activeDocument())
numdep = 0
self.retrieveDOFInfo(
) #function only once used here at this place in whole program
for rig in self.rigids:
rig.currentDOF()
rig.beautyDOFPrint()
numdep += rig.countDependencies()
Msg('there are {} dependencies\n'.format(numdep / 2))
def solveSystem(self, doc):
Msg("\n===== Start Solving System ====== \n")
systemSolved = self.solveAccuracySteps(doc)
if self.status == "loadingDependencyError":
return
if systemSolved:
self.status = "solved"
Msg("===== System solved using partial + recursive unfixing =====\n"
)
else:
self.status = "unsolved"
Msg("===== Could not solve system ====== \n")
msg = \
'''
Constraints inconsistent. Cannot solve System.
Please delete your last created constraint !
'''
QtGui.QMessageBox.information(QtGui.QApplication.activeWindow(),
"Constraint mismatch", msg)
def solveSystem(self,doc):
Msg( "\n===== Start Solving System ====== \n" )
if a2plib.isPartialProcessing():
Msg( "Solvermode = partialProcessing !\n")
mode = 'partial'
else:
Msg( "Solvermode = solve all Parts at once !\n")
mode = 'magnetic'
systemSolved = self.solveSystemWithMode(doc,mode)
if self.status == "loadingDependencyError":
return
if not systemSolved and mode == 'partial':
Msg( "Could not solve system with partial processing, switch to 'magnetic' mode \n" )
mode = 'magnetic'
systemSolved = self.solveSystemWithMode(doc,mode)
if systemSolved:
self.status = "solved"
Msg( "===== System solved ! ====== \n" )
else:
self.status = "unsolved"
Msg( "===== Could not solve system ====== \n" )
msg = \
'''
Constraints inconsistent. Cannot solve System.
Please delete your last created constraint !
'''
QtGui.QMessageBox.information(
QtGui.QApplication.activeWindow(),
"Constraint mismatch",
msg
)
def beautyDOFPrint(self):
'''
pretty print output that describe the current DOF of the rigid
'''
Msg('\n')
Msg(u"Current Rigid = {}\n".format(self.label))
if self.fixed:
Msg(u" is Fixed\n")
else:
Msg(u" is not Fixed and has {} DegreesOfFreedom\n".format(
self.currentDOF()))
for rig in self.depsPerLinkedRigids.keys():
Msg(u" Depends on Rigid = {}\n".format(rig.label))
for dep in self.depsPerLinkedRigids[rig]:
Msg(u" {}\n".format(dep))
Msg(u" DOF Position free with this rigid = {}\n".format(
len(self.dofPOSPerLinkedRigids[rig])))
Msg(u" DOF Rotation free with this rigid = {}\n".format(
len(self.dofROTPerLinkedRigids[rig])))
def visualizeHierarchy(self):
home = expanduser("~")
out_file = os.path.join(home,'assembly_hierarchy.html')
Msg("Writing visual hierarchy to: {}\n".format(out_file))
f = open(out_file, "w")
f.write("<!DOCTYPE html>\n")
f.write("<html>\n")
f.write("<head>\n")
f.write(' <meta charset="utf-8">\n')
f.write(' <meta http-equiv="X-UA-Compatible" content="IE=edge">\n')
f.write(' <title>A2P assembly hierarchy visualization</title>\n')
f.write("</head>\n")
f.write("<body>\n")
f.write('<div class="mermaid">\n')
f.write("graph TD\n")
for rig in self.rigids:
# No children, add current rogod as a leaf entry
if len(rig.childRigids) == 0:
f.write("{}\n".format(rig.label))
else:
# Rigid have children, add them based on the dependency list
for d in rig.dependencies:
if d.dependedRigid in rig.childRigids:
if rig.fixed:
f.write("{}({}<br>*FIXED*) -- {} --> {}\n".format(rig.label, rig.label, d.Type, d.dependedRigid.label))
else:
f.write("{} -- {} --> {}\n".format(rig.label, d.Type, d.dependedRigid.label))
f.write("</div>\n")
f.write(' <script src="https://unpkg.com/[email protected]/dist/mermaid.js"></script>\n')
f.write(" <script>\n")
f.write(' mermaid.initialize({startOnLoad: true});\n')
f.write(" </script>\n")
f.write("</body>")
f.write("</html>")
f.close()
def calculateWorkList(self, doc, workList):
reqPosAccuracy = self.mySOLVER_POS_ACCURACY
reqSpinAccuracy = self.mySOLVER_SPIN_ACCURACY
for rig in workList:
rig.enableDependencies(workList)
for rig in workList:
rig.calcSpinBasicDataDepsEnabled()
self.lastPositionError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.lastAxisError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.convergencyCounter = 0
calcCount = 0
goodAccuracy = False
while not goodAccuracy:
maxPosError = 0.0
maxAxisError = 0.0
maxSingleAxisError = 0.0
calcCount += 1
self.stepCount += 1
self.convergencyCounter += 1
# First calculate all the movement vectors
for w in workList:
w.moved = True
w.calcMoveData(doc, self)
if w.maxPosError > maxPosError:
maxPosError = w.maxPosError
if w.maxAxisError > maxAxisError:
maxAxisError = w.maxAxisError
if w.maxSingleAxisError > maxSingleAxisError:
maxSingleAxisError = w.maxSingleAxisError
# Perform the move
for w in workList:
w.move(doc)
# The accuracy is good, apply the solution to FreeCAD's objects
if (maxPosError <= reqPosAccuracy and # relevant check
maxAxisError <= reqSpinAccuracy and # relevant check
maxSingleAxisError <= reqSpinAccuracy *
10 # additional check for insolvable assemblies
# sometimes spin can be solved but singleAxis not..
):
# The accuracy is good, we're done here
goodAccuracy = True
# Mark the rigids as tempfixed and add its constrained rigids to pending list to be processed next
for r in workList:
r.applySolution(doc, self)
r.tempfixed = True
if self.convergencyCounter > SOLVER_STEPS_CONVERGENCY_CHECK:
if (maxPosError >=
SOLVER_CONVERGENCY_FACTOR * self.lastPositionError
or maxAxisError >=
SOLVER_CONVERGENCY_FACTOR * self.lastAxisError):
foundRigidToUnfix = False
# search for unsolved dependencies...
for rig in workList:
if rig.fixed or rig.tempfixed: continue
#if rig.maxAxisError >= maxAxisError or rig.maxPosError >= maxPosError:
if rig.maxAxisError > reqSpinAccuracy or rig.maxPosError > reqPosAccuracy:
for r in rig.linkedRigids:
if r.tempfixed and not r.fixed:
r.tempfixed = False
#Msg("unfixed Rigid {}\n".format(r.label))
foundRigidToUnfix = True
if foundRigidToUnfix:
self.lastPositionError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.lastAxisError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.convergencyCounter = 0
continue
else:
Msg('\n')
Msg('convergency-conter: {}\n'.format(
self.convergencyCounter))
Msg("Calculation stopped, no convergency anymore!\n")
return False
self.lastPositionError = maxPosError
self.lastAxisError = maxAxisError
self.maxSingleAxisError = maxSingleAxisError
self.convergencyCounter = 0
if self.stepCount > SOLVER_MAXSTEPS:
Msg("Reached max calculations count ({})\n".format(
SOLVER_MAXSTEPS))
return False
return True
def printList(self, name, l):
Msg("{} = (".format(name))
for e in l:
Msg("{} ".format(e.label))
Msg("):\n")
def DOF_info_to_console(self):
doc = FreeCAD.activeDocument()
dofGroup = doc.getObject("dofLabels")
if dofGroup == None:
dofGroup = doc.addObject("App::DocumentObjectGroup", "dofLabels")
else:
for lbl in dofGroup.Group:
doc.removeObject(lbl.Name)
doc.removeObject("dofLabels")
dofGroup = doc.addObject("App::DocumentObjectGroup", "dofLabels")
self.loadSystem(doc)
#look for unconstrained objects and label them
solverObjectNames = []
for rig in self.rigids:
solverObjectNames.append(rig.objectName)
shapeObs = a2plib.filterShapeObs(doc.Objects)
for so in shapeObs:
if so.Name not in solverObjectNames:
ob = doc.getObject(so.Name)
if ob.ViewObject.Visibility == True:
bbCenter = ob.Shape.BoundBox.Center
dofLabel = doc.addObject("App::AnnotationLabel",
"dofLabel")
dofLabel.LabelText = "FREE"
dofLabel.BasePosition.x = bbCenter.x
dofLabel.BasePosition.y = bbCenter.y
dofLabel.BasePosition.z = bbCenter.z
#
dofLabel.ViewObject.BackgroundColor = a2plib.BLUE
dofLabel.ViewObject.TextColor = a2plib.WHITE
dofGroup.addObject(dofLabel)
numdep = 0
self.retrieveDOFInfo(
) #function only once used here at this place in whole program
for rig in self.rigids:
dofCount = rig.currentDOF()
ob = doc.getObject(rig.objectName)
if ob.ViewObject.Visibility == True:
bbCenter = ob.Shape.BoundBox.Center
dofLabel = doc.addObject("App::AnnotationLabel", "dofLabel")
if rig.fixed:
dofLabel.LabelText = "Fixed"
else:
dofLabel.LabelText = "DOFs: {}".format(dofCount)
dofLabel.BasePosition.x = bbCenter.x
dofLabel.BasePosition.y = bbCenter.y
dofLabel.BasePosition.z = bbCenter.z
if rig.fixed:
dofLabel.ViewObject.BackgroundColor = a2plib.RED
dofLabel.ViewObject.TextColor = a2plib.BLACK
elif dofCount == 0:
dofLabel.ViewObject.BackgroundColor = a2plib.RED
dofLabel.ViewObject.TextColor = a2plib.BLACK
elif dofCount < 6:
dofLabel.ViewObject.BackgroundColor = a2plib.YELLOW
dofLabel.ViewObject.TextColor = a2plib.BLACK
dofGroup.addObject(dofLabel)
rig.beautyDOFPrint()
numdep += rig.countDependencies()
Msg('there are {} dependencies\n'.format(numdep / 2))
def printHierarchy(self, level):
Msg((level * 3) * " ")
Msg("{} - distance {}\n".format(self.label, self.disatanceFromFixed))
for rig in self.childRigids:
rig.printHierarchy(level + 1)
def calculateWorkList(self, doc, workList, mode):
if A2P_DEBUG_LEVEL >= A2P_DEBUG_1:
self.printList("WorkList", workList)
for rig in workList:
rig.enableDependencies(workList)
self.lastPositionError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.lastAxisError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.convergencyCounter = 0
calcCount = 0
goodAccuracy = False
while not goodAccuracy:
maxPosError = 0.0
maxAxisError = 0.0
calcCount += 1
self.stepCount += 1
self.convergencyCounter += 1
# First calculate all the movement vectors
for w in workList:
w.calcMoveData(doc, self)
if w.maxPosError > maxPosError:
maxPosError = w.maxPosError
if w.maxAxisError > maxAxisError:
maxAxisError = w.maxAxisError
# Perform the move
for w in workList:
w.move(doc)
# Enable those 2 lines to see the computation progress on screen
#w.applySolution(doc, self)
#FreeCADGui.updateGui()
# The accuracy is good, apply the solution to FreeCAD's objects
if (maxPosError <= self.mySOLVER_POS_ACCURACY and
maxAxisError <= self.mySOLVER_SPIN_ACCURACY):
# The accuracy is good, we're done here
goodAccuracy = True
# Mark the rigids as tempfixed and add its constrained rigids to pending list to be processed next
DebugMsg(A2P_DEBUG_1, "{} counts \n".format(calcCount) )
for r in workList:
r.applySolution(doc, self)
r.tempfixed = True
if self.convergencyCounter > SOLVER_STEPS_CONVERGENCY_CHECK:
if (
maxPosError >= self.lastPositionError or
maxAxisError >= self.lastAxisError
):
if mode == 'magnetic':
Msg( "System not solvable, convergency is incorrect!\n" )
return False
self.lastPositionError = maxPosError
self.lastAxisError = maxAxisError
self.convergencyCounter = 0
if self.stepCount > SOLVER_MAXSTEPS:
if mode == 'magnetic':
Msg( "Reached max calculations count ({})\n".format(SOLVER_MAXSTEPS) )
return False
return True
