405 lines
15 KiB
Python
405 lines
15 KiB
Python
# Алгоритм генерации графа с помощью оценки стабильности подсборок в физическом движке PyBullet
|
||
from typing import Any, TypeVar, Type, cast
|
||
import FreeCAD as App
|
||
import json
|
||
import importOBJ
|
||
import FreeCAD as App
|
||
import Draft
|
||
import os
|
||
import Part
|
||
import numpy as np
|
||
from typing import TypeAlias
|
||
import FreeCADGui as Gui
|
||
|
||
ISequencesUnion: TypeAlias = dict[str:dict[str:list[str]]]
|
||
|
||
|
||
def importObjAtPath(path: str, inc: int):
|
||
importOBJ.insert(u"" + path, App.ActiveDocument.Label)
|
||
|
||
mesh = App.ActiveDocument.Objects[inc]
|
||
shape = Part.Shape()
|
||
shape.makeShapeFromMesh(mesh.Mesh.Topology, 0.05)
|
||
solid = Part.makeSolid(shape)
|
||
Part.show(solid)
|
||
App.ActiveDocument.Objects[inc +
|
||
1].Label = App.ActiveDocument.Objects[inc].Name
|
||
App.ActiveDocument.removeObject(App.ActiveDocument.Objects[inc].Name)
|
||
return App.ActiveDocument.Objects[inc]
|
||
|
||
|
||
T = TypeVar("T")
|
||
|
||
|
||
def from_float(x: Any) -> float:
|
||
assert isinstance(x, (float, int)) and not isinstance(x, bool)
|
||
return float(x)
|
||
|
||
|
||
def to_float(x: Any) -> float:
|
||
assert isinstance(x, float)
|
||
return x
|
||
|
||
|
||
def from_str(x: Any) -> str:
|
||
assert isinstance(x, str)
|
||
return x
|
||
|
||
|
||
def to_class(c: Type[T], x: Any) -> dict:
|
||
assert isinstance(x, c)
|
||
return cast(Any, x).to_dict()
|
||
|
||
|
||
def euler_to_quaternion(yaw, pitch, roll):
|
||
|
||
qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - \
|
||
np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
|
||
qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + \
|
||
np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
|
||
qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - \
|
||
np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
|
||
qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + \
|
||
np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
|
||
|
||
return [qx, qy, qz, qw]
|
||
|
||
|
||
class Coords:
|
||
x: float
|
||
y: float
|
||
z: float
|
||
|
||
def __init__(self, x: float, y: float, z: float) -> None:
|
||
self.x = x
|
||
self.y = y
|
||
self.z = z
|
||
|
||
@staticmethod
|
||
def from_dict(obj: Any) -> 'Coords':
|
||
assert isinstance(obj, dict)
|
||
x = from_float(obj.get("x"))
|
||
y = from_float(obj.get("y"))
|
||
z = from_float(obj.get("z"))
|
||
return Coords(x, y, z)
|
||
|
||
def to_dict(self) -> dict:
|
||
result: dict = {}
|
||
result["x"] = to_float(self.x)
|
||
result["y"] = to_float(self.y)
|
||
result["z"] = to_float(self.z)
|
||
return result
|
||
|
||
def vector(self):
|
||
return App.Vector(self.x, self.y, self.z)
|
||
|
||
|
||
class MotionResultModel:
|
||
id: str
|
||
euler: Coords
|
||
position: Coords
|
||
|
||
def __init__(self, id: str, euler: Coords, position: Coords) -> None:
|
||
self.id = id
|
||
self.euler = euler
|
||
self.position = position
|
||
|
||
@staticmethod
|
||
def from_dict(obj: Any) -> 'MotionResultModel':
|
||
assert isinstance(obj, dict)
|
||
id = from_str(obj.get("id"))
|
||
euler = Coords.from_dict(obj.get("euler"))
|
||
position = Coords.from_dict(obj.get("position"))
|
||
return MotionResultModel(id, euler, position)
|
||
|
||
def to_dict(self) -> dict:
|
||
result: dict = {}
|
||
result["id"] = from_str(self.id)
|
||
result["euler"] = to_class(Coords, self.euler)
|
||
result["position"] = to_class(Coords, self.position)
|
||
return result
|
||
|
||
|
||
class SequencesEvaluation:
|
||
sequences: ISequencesUnion
|
||
assemblyDir: str
|
||
result: dict = {}
|
||
|
||
def __init__(self, sequences, assemblyDir) -> None:
|
||
self.sequences = sequences
|
||
self.assemblyDir = assemblyDir
|
||
pass
|
||
|
||
def assemblyComputed(self):
|
||
debug = True
|
||
for sequenceNumber, v in self.sequences.items():
|
||
for assemblyNumber, assemblySequenced in v.items():
|
||
# print(assemblyNumber)
|
||
# print()
|
||
# if(assemblyNumber == 1 and sequenceNumber == 4 or assemblyNumber == 1 and sequenceNumber == 0):
|
||
if(assemblyNumber == 1 and sequenceNumber == 1 and debug):
|
||
debug = False
|
||
if(sequenceNumber == 0):
|
||
sequenceNumber+=1
|
||
print(assemblySequenced)
|
||
self.comptedAssembly(
|
||
assemblySequenced, sequenceNumber, assemblyNumber)
|
||
|
||
pass
|
||
|
||
def comptedAssembly(self, assembly: list[str], sequenceNumber: int, assemblyNumber: int):
|
||
assemblyParts = []
|
||
for counter in range(len(assembly)):
|
||
importObjAtPath(
|
||
self.assemblyDir + 'sdf/meshes/' + assembly[counter] + '.obj',
|
||
counter
|
||
)
|
||
assemblyParts.append({
|
||
"part": App.ActiveDocument.Objects[counter],
|
||
"name": assembly[counter]
|
||
})
|
||
|
||
motionResult = json.loads((open(self.assemblyDir + 'stability' + '/' + str(
|
||
sequenceNumber + 1) + '/' + str(assemblyNumber) + '/' + 'motion_result.json')).read())
|
||
|
||
simulatorMotionResults: list['MotionResultModel'] = []
|
||
for _k, v in motionResult.items():
|
||
simulatorMotionResults.append(MotionResultModel.from_dict(v))
|
||
|
||
for el in simulatorMotionResults:
|
||
for e in assemblyParts:
|
||
# сопоставляем детали
|
||
if (el.id == e.get('name')):
|
||
# вычисляем центр детали для перемещения
|
||
center = e.get('part').Shape.CenterOfMass
|
||
# получаем центр деталей из симуляции
|
||
new_center = App.Vector(el.position.vector())
|
||
|
||
# вычисляем вектор смещения
|
||
offset = new_center - center
|
||
# перемещаем деталь на вектор смещения
|
||
e.get('part').Placement.Base += offset
|
||
|
||
# импортируем меш связанный с зоной обьекта
|
||
# zonePart = importObjAtPath(self.assemblyDir + "stability/zones/meshes/zone_sub_assembly" + str(
|
||
# assembly.__len__() - 1) + ".obj", len(App.ActiveDocument.Objects))
|
||
# получаем координаты зоны относительно детали за которой закреплена зона
|
||
print(assemblyNumber)
|
||
coords = json.loads(
|
||
(open(self.assemblyDir + "stability/zones/sub_assembly_coords_" + str(assemblyNumber - 1) + ".json")).read())
|
||
assemblyCounter = len(assemblyParts)
|
||
detailWhichZoneBindings = assemblyParts[assemblyCounter - 2].get(
|
||
'part')
|
||
|
||
detailStabilityComputed = assemblyParts[assemblyCounter - 1].get(
|
||
'part')
|
||
relativeCoordinates = coords.get('relativeCoordinates')
|
||
|
||
relativeEuler = coords.get('relativeEuler')
|
||
|
||
specificEuler = {
|
||
'yaw': detailWhichZoneBindings.Placement.Rotation.toEuler()[0] - relativeEuler.get('yaw'),
|
||
'pitch': detailWhichZoneBindings.Placement.Rotation.toEuler()[1] - relativeEuler.get('pitch'),
|
||
'roll': detailWhichZoneBindings.Placement.Rotation.toEuler()[2] - relativeEuler.get('roll')
|
||
}
|
||
quaternion = euler_to_quaternion(specificEuler.get(
|
||
'yaw'), specificEuler.get('pitch'), specificEuler.get('roll'))
|
||
rotation = App.Rotation(
|
||
quaternion[0], quaternion[1], quaternion[2], quaternion[3])
|
||
|
||
detailStabilityComputed.Placement.Rotation = rotation
|
||
centerVector = detailWhichZoneBindings.Shape.CenterOfMass
|
||
vector = App.Vector(relativeCoordinates.get(
|
||
'x'), relativeCoordinates.get('y'), relativeCoordinates.get('z'))
|
||
|
||
# current_center = zonePart.Shape.CenterOfMass
|
||
# move_vector = App.Vector(centerVector + vector) - current_center
|
||
# zonePart.Placement.move(move_vector)
|
||
|
||
# computedStabilityResult = computedStability(
|
||
# zonePart, detailStabilityComputed)
|
||
|
||
if sequenceNumber not in self.result.keys():
|
||
self.result[sequenceNumber] = []
|
||
|
||
# self.result[sequenceNumber].append({
|
||
# str(assemblyNumber): assembly,
|
||
# "result": computedStabilityResult
|
||
# })
|
||
|
||
# for part in App.ActiveDocument.Objects:
|
||
# App.ActiveDocument.removeObject(part.Name)
|
||
|
||
|
||
def get_part_center(part):
|
||
shape = None
|
||
if not hasattr(part, 'Shape'):
|
||
shape = part.Mesh
|
||
if hasattr(part, 'Shape'):
|
||
shape = part.Shape
|
||
|
||
center = shape.BoundBox.Center
|
||
return App.Vector(center[0],
|
||
center[1],
|
||
center[2])
|
||
|
||
|
||
def move_second_part_to_match_center(first_part, second_part):
|
||
first_center = get_part_center(first_part)
|
||
second_center = get_part_center(second_part)
|
||
offset = first_center - second_center
|
||
second_part.Placement.move(offset)
|
||
|
||
|
||
def create(part):
|
||
clone = Draft.make_clone([part], forcedraft=True)
|
||
|
||
clone.Scale = App.Vector(1.30, 1.30, 1.30)
|
||
clone_corr = (App.Vector(0.4476673941774023, -2.109332894191716, -0.5918687740295264) -
|
||
clone.Placement.Base).scale(*App.Vector(-0.25, -0.25, -0.25))
|
||
clone.Placement.move(clone_corr)
|
||
App.ActiveDocument.recompute()
|
||
|
||
return clone
|
||
|
||
|
||
def getFullPathObj(assemblyFolder: str, name: str):
|
||
return assemblyFolder + 'sdf/meshes/' + name + '.obj'
|
||
|
||
|
||
def computedStability(refElement, childElement):
|
||
rootElement = childElement.Shape.BoundBox
|
||
# Создание обьекта на котором делается операция пересечения
|
||
App.activeDocument().addObject("Part::MultiCommon", "Common")
|
||
App.activeDocument().Common.Shapes = [refElement, childElement, ]
|
||
App.ActiveDocument.getObject('Common').ViewObject.ShapeColor = getattr(App.ActiveDocument.getObject(
|
||
refElement.Name).getLinkedObject(True).ViewObject, 'ShapeColor', App.ActiveDocument.getObject('Common').ViewObject.ShapeColor)
|
||
App.ActiveDocument.getObject('Common').ViewObject.DisplayMode = getattr(App.ActiveDocument.getObject(
|
||
childElement.Name).getLinkedObject(True).ViewObject, 'DisplayMode', App.ActiveDocument.getObject('Common').ViewObject.DisplayMode)
|
||
App.ActiveDocument.recompute()
|
||
obj = App.ActiveDocument.getObjectsByLabel('Common')[0]
|
||
|
||
shp = obj.Shape
|
||
bbox = shp.BoundBox
|
||
# Если после операции пересечения зона обьекта совпадает с зоной тестируемого обьекта то тест прошел успешно
|
||
if bbox.XLength == rootElement.XLength and bbox.YLength == rootElement.YLength and rootElement.ZLength == bbox.ZLength:
|
||
return True
|
||
return False
|
||
|
||
|
||
def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPath: str):
|
||
|
||
cadObjects = []
|
||
|
||
for count in range(len(stepFilesPaths)):
|
||
|
||
importObjAtPath(stepFilesPaths[count], count)
|
||
cadObjects.append(App.ActiveDocument.Objects[count])
|
||
|
||
assemblesBindings = []
|
||
|
||
for increment in range(len(cadObjects)):
|
||
if (increment != 0):
|
||
detailForEvaluationZ = cadObjects[increment]
|
||
zoneBindingDetailZ = cadObjects[increment-1]
|
||
assemblesBindings.append(
|
||
{'zoneBindingDetail': detailForEvaluationZ, 'detailForEvaluation': zoneBindingDetailZ, 'relativeCoordinates': None, 'zonePart': None})
|
||
for increment in range(len(assemblesBindings)):
|
||
|
||
el = assemblesBindings[increment]
|
||
|
||
zoneBindingDetail = el.get('zoneBindingDetail')
|
||
zoneBindingDetailCenterVector = zoneBindingDetail.Shape.CenterOfMass
|
||
zoneDetail = create(el.get('detailForEvaluation'))
|
||
|
||
move_second_part_to_match_center(
|
||
el.get('zoneBindingDetail'), zoneDetail)
|
||
zoneDetail.Label = 'zone_sub_assembly' + str(increment + 1)
|
||
|
||
zoneDetail.ViewObject.ShapeColor = (0.40, 0.74, 0.71)
|
||
zoneDetail.ViewObject.Transparency = 50
|
||
zoneDetailCenterVector = el.get(
|
||
'detailForEvaluation').Shape.CenterOfMass
|
||
|
||
el['relativeCoordinates'] = {
|
||
'x': zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x,
|
||
'y': zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y,
|
||
'z': zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z
|
||
}
|
||
|
||
el['relativeEuler'] = {
|
||
'yaw': zoneBindingDetail.Placement.Rotation.toEuler()[0] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[0],
|
||
'pitch': zoneBindingDetail.Placement.Rotation.toEuler()[1] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[1],
|
||
'roll': zoneBindingDetail.Placement.Rotation.toEuler()[2] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[2]
|
||
}
|
||
el['zonePart'] = zoneDetail
|
||
|
||
meshesPath = directoryStableZonesPath + 'meshes/'
|
||
if not os.path.exists(directoryStableZonesPath):
|
||
os.makedirs(directoryStableZonesPath)
|
||
if not os.path.exists(meshesPath):
|
||
os.makedirs(meshesPath)
|
||
zonesSaved = {}
|
||
for counter in range(len(assemblesBindings)):
|
||
zoneComputed = assemblesBindings[counter]
|
||
mesh = zoneComputed.get('zonePart')
|
||
zonesSavePath = meshesPath + mesh.Label + '.obj'
|
||
importOBJ.export([mesh], zonesSavePath)
|
||
zonesSaved[mesh.Label] = 'meshes/' + mesh.Label + '.obj'
|
||
for counter in range(len(assemblesBindings)):
|
||
el = assemblesBindings[counter]
|
||
savePath = zonesSaved[el.get('zonePart').Label]
|
||
el['zonePart'] = savePath
|
||
el['detailForEvaluation'] = el['detailForEvaluation'].Label
|
||
el['zoneBindingDetail'] = el['zoneBindingDetail'].Label
|
||
|
||
json_result = json.dumps(el)
|
||
|
||
file_to_open = directoryStableZonesPath + \
|
||
'sub_assembly_coords_' + str(counter) + '.json'
|
||
|
||
f = open(file_to_open, 'w', )
|
||
|
||
f.write(json_result)
|
||
f.close()
|
||
|
||
|
||
def main():
|
||
App.newDocument()
|
||
env = json.loads((open('./env.json')).read())
|
||
|
||
assemblyDir = env.get('aspPath')
|
||
sequencesJSON = json.loads((open(assemblyDir + 'sequences.json')).read())
|
||
directoryStableZones = assemblyDir + 'stability/zones/'
|
||
|
||
sequences = sequencesJSON.get('sequences')
|
||
stepStructure = json.loads(
|
||
(open(assemblyDir + 'step-structure.json')).read())
|
||
|
||
stepFilesPaths = []
|
||
for step in stepStructure:
|
||
stepFilesPaths.append(assemblyDir+'sdf/meshes/' + step + '.obj')
|
||
if not os.path.exists(directoryStableZones):
|
||
print('Zones not found automatic calculation started')
|
||
autoStabilityZoneComputed(stepFilesPaths, directoryStableZones)
|
||
|
||
sequencesJoin = {}
|
||
|
||
for arrayCounter in range(len(sequences)):
|
||
for indexCounter in range(len(sequences[arrayCounter])):
|
||
if (indexCounter != 0):
|
||
if (sequencesJoin.get(arrayCounter) == None):
|
||
sequencesJoin[arrayCounter] = {
|
||
indexCounter: sequences[arrayCounter][0:indexCounter+1]
|
||
}
|
||
else:
|
||
sequencesJoin[arrayCounter][indexCounter] = sequences[arrayCounter][0:indexCounter+1]
|
||
|
||
|
||
|
||
seqEvaluation = SequencesEvaluation(sequencesJoin, assemblyDir)
|
||
seqEvaluation.assemblyComputed()
|
||
print(seqEvaluation.result)
|
||
|
||
|
||
main()
|