adding comments
This commit is contained in:
IDONTSUDO
parent
9eaf12a4fa
commit
12482ff40f
7 changed files with 102 additions and 75 deletions
|
|
@ -1,7 +1,6 @@
|
|||
|
||||
from usecases.export_assembly_them_all_usecase import ExportAssemblyThemAllUseCase
|
||||
import FreeCAD
|
||||
|
||||
from usecases.export_usecase import EXPORT_TYPES, ExportUseCase
|
||||
from usecases.get_sdf_geometry_usecase import SdfGeometryUseCase
|
||||
from usecases.assembly_parse_usecase import AssemblyParseUseCase
|
||||
|
|
@ -10,7 +9,6 @@ from model.geometry_part import GeometryPart
|
|||
from model.files_generator import FolderGenerator
|
||||
from helper.fs import FS
|
||||
import os
|
||||
# import ImportGui
|
||||
import shutil
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ import json
|
|||
import re
|
||||
from pyquaternion import Quaternion
|
||||
|
||||
|
||||
def importObjAtPath(path: str):
|
||||
import importOBJ
|
||||
importOBJ.insert(u"" + path, App.ActiveDocument.Label)
|
||||
|
|
@ -14,7 +15,7 @@ def getFullPathObj(assemblyFolder: str, name: str):
|
|||
return assemblyFolder + 'sdf/meshes/' + name + '.obj'
|
||||
|
||||
|
||||
def computedStabiliti(refElement, childElement):
|
||||
def computedStability(refElement, childElement):
|
||||
b = childElement.Shape.BoundBox
|
||||
App.activeDocument().addObject("Part::MultiCommon", "Common")
|
||||
App.activeDocument().Common.Shapes = [refElement, childElement, ]
|
||||
|
|
@ -34,16 +35,24 @@ def computedStabiliti(refElement, childElement):
|
|||
|
||||
def main():
|
||||
App.newDocument()
|
||||
# загрузка env интерфейса
|
||||
env = json.loads((open('./env.json')).read())
|
||||
coordinatsFilePath = env.get('pathToTheSimulationCoordinatesFile')
|
||||
assemblyFolder = env.get('generationFolder')
|
||||
buildNumber = int(re.findall(r'\d', coordinatsFilePath)[0])
|
||||
|
||||
# получение пути координат файла
|
||||
coordinatesFilePath = env.get('pathToTheSimulationCoordinatesFile')
|
||||
|
||||
# получение папки сборки
|
||||
assemblyFolder = env.get('generationFolder')
|
||||
buildNumber = int(re.findall(r'\d', coordinatesFilePath)[0])
|
||||
|
||||
# загрузка последовательности сборки ввиде списка строк деталей
|
||||
assemblyStructure = json.loads(
|
||||
(open(assemblyFolder + 'step-structure.json')).read())
|
||||
# получение номера тестируемой сборки
|
||||
assemblyNumber = int(buildNumber)
|
||||
# получение тестируемой детали в сборке
|
||||
activeDetail = assemblyStructure[assemblyNumber]
|
||||
|
||||
|
||||
subassemblyNotParticipatingInMarkup = assemblyStructure[0:assemblyNumber - 1]
|
||||
detailOfTheMarkingZoneOfWhich = assemblyStructure[assemblyNumber - 1]
|
||||
importObjAtPath(getFullPathObj(assemblyFolder, activeDetail))
|
||||
|
|
@ -57,22 +66,24 @@ def main():
|
|||
importObjAtPath(el)
|
||||
for el in App.ActiveDocument.Objects[2:App.ActiveDocument.Objects.__len__()]:
|
||||
el.ViewObject.ShapeColor = (0.32, 0.05, 0.38)
|
||||
|
||||
coordinats = json.loads((open(coordinatsFilePath)).read())
|
||||
# загрузка координат
|
||||
coordinates = json.loads((open(coordinatesFilePath)).read())
|
||||
inc = 1
|
||||
# перемещение обьектов в документе на точки стабильность полученные из pybullet
|
||||
for el in App.ActiveDocument.Objects:
|
||||
pos = coordinats[inc]['position']
|
||||
qua = coordinats[inc]['quaternion']
|
||||
pos = coordinates[inc]['position']
|
||||
qua = coordinates[inc]['quaternion']
|
||||
new_quaternion = Quaternion(qua[0], qua[1], qua[2], qua[3])
|
||||
rotation_matrix = new_quaternion.rotation_matrix
|
||||
current_position = el.Placement.Base
|
||||
new_position = App.Vector(current_position.x, current_position.y, current_position.z)
|
||||
new_position = App.Vector(
|
||||
current_position.x, current_position.y, current_position.z)
|
||||
new_placement = App.Placement(new_position, rotation_matrix)
|
||||
el.Placement = new_placement
|
||||
App.ActiveDocument.recompute()
|
||||
el.Placement.move(App.Vector(pos[0], pos[1], pos[2]))
|
||||
|
||||
print(computedStabiliti(meshMark, meshDetailOfTheMarkZone))
|
||||
# вычисление стабильность
|
||||
computedStability(meshMark, meshDetailOfTheMarkZone)
|
||||
pass
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@ def main():
|
|||
App.newDocument()
|
||||
env = json.loads((open('./env.json')).read())
|
||||
assemblyFolder = env.get('assemblyFolder')
|
||||
|
||||
assemblyStructure = json.loads(
|
||||
(open(assemblyFolder + 'step-structure.json')).read())
|
||||
assemblyNumber = int(env.get('buildNumber')) - 1
|
||||
|
|
|
|||
|
|
@ -245,7 +245,7 @@ class GetPartPrimitiveCoordinatesUseCase(object):
|
|||
|
||||
|
||||
class InitPartsParseUseCase():
|
||||
|
||||
# Инициализация парсинга
|
||||
def call(self):
|
||||
product_details = []
|
||||
for part in FreeCadRepository().getAllSolids():
|
||||
|
|
@ -257,13 +257,14 @@ class InitPartsParseUseCase():
|
|||
|
||||
|
||||
class RenderPrimitiveUseCase(object):
|
||||
|
||||
# Рендеринг премитивов
|
||||
def call(self, meshModels: list[MeshGeometryCoordinateModel], detailSquares) -> None:
|
||||
for mesh in meshModels:
|
||||
mesh.initializePrimitivesByCoordinate(detailSquares)
|
||||
|
||||
|
||||
class ClearWorkSpaceDocumentUseCase(object):
|
||||
# Очистка рабочего простарнства
|
||||
def call(self, detailSquares):
|
||||
for key in detailSquares:
|
||||
for renderPrimitive in detailSquares[key]:
|
||||
|
|
@ -309,7 +310,9 @@ class ClearWorkSpaceDocumentUseCase(object):
|
|||
|
||||
|
||||
class CadAdjacencyMatrix:
|
||||
# Матрица основанная на соприкосновении примитива с обьектами
|
||||
def primitiveMatrix(self):
|
||||
# Получение матрицы
|
||||
matrix = RenderPrimitivesScenario(
|
||||
InitPartsParseUseCase(),
|
||||
GetPartPrimitiveCoordinatesUseCase(),
|
||||
|
|
@ -317,27 +320,27 @@ class CadAdjacencyMatrix:
|
|||
GetCollisionAtPrimitiveUseCase(),
|
||||
ClearWorkSpaceDocumentUseCase(),
|
||||
).call()
|
||||
|
||||
return AdjacencyMatrix(
|
||||
all_parts=GetAllPartsLabelsUseCase().call(),
|
||||
first_detail=GetFirstDetailUseCase().call(),
|
||||
matrix=matrix,
|
||||
)
|
||||
|
||||
def matrixBySurfaces(self,):
|
||||
adjaxed = {}
|
||||
# Матрица основанная на соприкосновении обьектов
|
||||
def matrixBySurfaces(self):
|
||||
matrix = {}
|
||||
for part in FreeCadRepository().getAllSolids():
|
||||
adjaxed[part.Label] = []
|
||||
matrix[part.Label] = []
|
||||
for nextPart in FreeCadRepository().getAllSolids():
|
||||
if part.Label != nextPart.Label:
|
||||
# Вычисление соприконсоновения площади деталей
|
||||
collisionResult: int = int(
|
||||
part.Shape.distToShape(nextPart.Shape)[0])
|
||||
if (collisionResult == 0):
|
||||
adjaxed[part.Label].append(nextPart.Label)
|
||||
matrix[part.Label].append(nextPart.Label)
|
||||
|
||||
return AdjacencyMatrix(all_parts=GetAllPartsLabelsUseCase(
|
||||
).call(), first_detail=GetFirstDetailUseCase().call(),
|
||||
matrix=adjaxed
|
||||
matrix=matrix
|
||||
)
|
||||
|
||||
|
||||
|
|
@ -349,9 +352,10 @@ def main():
|
|||
return TypeError('CadFile not found env.json')
|
||||
App.open(u'' + cadFile)
|
||||
|
||||
|
||||
# Получение матрицы
|
||||
matrixOut = CadAdjacencyMatrix().primitiveMatrix().to_dict()
|
||||
import json
|
||||
# Запись результата
|
||||
FS.writeFile(json.dumps(matrixOut, ensure_ascii=False, indent=4), outPath,'out.json')
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,25 +1,25 @@
|
|||
from scipy.spatial.transform import Rotation
|
||||
import shutil
|
||||
from spatialmath import *
|
||||
from spatialmath.base import *
|
||||
from assembly.assets.process_mesh import process_mesh
|
||||
from assembly.examples.run_joint_plan import get_planner
|
||||
from assembly.baselines.run_joint_plan import PyPlanner
|
||||
from assembly.assets.subdivide import subdivide_to_size
|
||||
import numpy as np
|
||||
import json
|
||||
import sys
|
||||
import os
|
||||
os.environ['OMP_NUM_THREADS'] = '1'
|
||||
import sys
|
||||
|
||||
project_base_dir = os.path.abspath(os.path.join(os.path.dirname(os.path.abspath(__file__)), './')) + '/assembly/'
|
||||
project_base_dir = os.path.abspath(os.path.join(
|
||||
os.path.dirname(os.path.abspath(__file__)), './')) + '/assembly/'
|
||||
|
||||
|
||||
sys.path.append(project_base_dir)
|
||||
sys.path.append(project_base_dir + '/baselines/')
|
||||
sys.path.append(project_base_dir + '/assets/')
|
||||
|
||||
import json
|
||||
import numpy as np
|
||||
from assembly.assets.subdivide import subdivide_to_size
|
||||
from assembly.baselines.run_joint_plan import PyPlanner
|
||||
from assembly.examples.run_joint_plan import get_planner
|
||||
from assembly.assets.process_mesh import process_mesh
|
||||
|
||||
from spatialmath.base import *
|
||||
from spatialmath import *
|
||||
import shutil
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
class FS:
|
||||
def readJSON(path: str):
|
||||
|
|
@ -38,9 +38,10 @@ class FS:
|
|||
|
||||
def readFilesTypeFolder(pathFolder: str, fileType='.json'):
|
||||
return os.listdir(pathFolder)
|
||||
|
||||
def readFolder(pathFolder: str):
|
||||
return list( map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder) ))
|
||||
|
||||
return list(map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder)))
|
||||
|
||||
|
||||
def listGetFirstValue(iterable, default=False, pred=None):
|
||||
return next(filter(pred, iterable), default)
|
||||
|
|
@ -52,68 +53,80 @@ def filterModels(filterModels, filterModelsDescription):
|
|||
models.append(listGetFirstValue(
|
||||
filterModels, None, lambda x: x.name == el))
|
||||
return models
|
||||
def meshTransformation():
|
||||
|
||||
|
||||
def main():
|
||||
from argparse import ArgumentParser
|
||||
parser = ArgumentParser()
|
||||
parser.add_argument('--asp-path', type=str, required=True)
|
||||
args = parser.parse_args()
|
||||
aspDir = args.asp_dir
|
||||
if(aspDir == None):
|
||||
|
||||
# Коректировка пути до папки с генерацией ASP
|
||||
if (aspDir == None):
|
||||
args.print_helper()
|
||||
if(aspDir[aspDir.__len__() - 1] != '/'):
|
||||
if (aspDir[aspDir.__len__() - 1] != '/'):
|
||||
aspDir += '/'
|
||||
assemblys = FS.readFolder(aspDir + 'assembly')
|
||||
# Получение списка папок с .obj обьектами
|
||||
assembles = FS.readFolder(aspDir + 'assembly')
|
||||
assemblyDirNormalize = []
|
||||
for el in assemblys:
|
||||
for el in assembles:
|
||||
try:
|
||||
|
||||
process_mesh(source_dir=el, target_dir=el+'/process/',subdivide=el, verbose=True)
|
||||
# Пост обработка .obj обьектов
|
||||
process_mesh(source_dir=el, target_dir=el +
|
||||
'/process/', subdivide=el, verbose=True)
|
||||
assemblyDirNormalize.append(el + '/process/')
|
||||
except Exception as e:
|
||||
print('ERRROR:')
|
||||
print(e)
|
||||
|
||||
for el in assemblyDirNormalize:
|
||||
asset_folder = os.path.join(project_base_dir, aspDir)
|
||||
assembly_dir = os.path.join(asset_folder, el)
|
||||
planner = get_planner('bfs')(assembly_dir,assembly_dir, 0, [1], False, 'sdf', 0.05, 0.01, 100, 100, True)
|
||||
|
||||
|
||||
planner = get_planner('bfs')(assembly_dir, assembly_dir, 0, [
|
||||
1], False, 'sdf', 0.05, 0.01, 100, 100, True)
|
||||
|
||||
# Планирование пути
|
||||
status, t_plan, path = planner.plan(
|
||||
120, seed=1, return_path=True, render=False, record_path=None
|
||||
)
|
||||
coords = []
|
||||
|
||||
|
||||
for k in path:
|
||||
seMatrix = SE3(k)
|
||||
euler = seMatrix.eul()
|
||||
coord = seMatrix.A[0:3,3]
|
||||
coord = seMatrix.A[0:3, 3]
|
||||
rot = Rotation.from_euler('xyz', euler, degrees=True).as_quat()
|
||||
coords.append({ 'quadrelion': [rot[0], rot[1], rot[2], rot[3]], 'xyz':[coord[0], coord[1], coord[2]],'euler': [euler[0], euler[1], euler[2]]})
|
||||
coords.append({'quadrelion': [rot[0], rot[1], rot[2], rot[3]], 'xyz': [
|
||||
coord[0], coord[1], coord[2]], 'euler': [euler[0], euler[1], euler[2]]})
|
||||
# Запись пути в кортеж
|
||||
planingObject = {
|
||||
"time": t_plan,
|
||||
"insertion_path": coords,
|
||||
"status":status,
|
||||
}
|
||||
FS.writeFile(json.dumps(planingObject),el[0:el.__len__() - 8], 'insertion_path.json' )
|
||||
|
||||
"status": status,
|
||||
}
|
||||
# Запись результата планирования
|
||||
FS.writeFile(json.dumps(planingObject),
|
||||
el[0:el.__len__() - 8], 'insertion_path.json')
|
||||
|
||||
try:
|
||||
planner = PyPlanner(assembly_dir, 'process', still_ids=[1], )
|
||||
planner = PyPlanner(assembly_dir, 'process', still_ids=[1],)
|
||||
status, t_plan, path = planner.plan(
|
||||
planner_name=args.planner,
|
||||
step_size=args.step_size,
|
||||
max_time=args.max_time,
|
||||
seed=args.seed,
|
||||
return_path=True,
|
||||
simplify=args.simplify,
|
||||
planner_name=args.planner,
|
||||
step_size=args.step_size,
|
||||
max_time=args.max_time,
|
||||
seed=args.seed,
|
||||
return_path=True,
|
||||
simplify=args.simplify,
|
||||
render=args.render
|
||||
)
|
||||
|
||||
print(f'Status: {status}, planning time: {t_plan}')
|
||||
|
||||
if args.save_dir is not None:
|
||||
planner.save_path(path, args.save_dir, args.n_save_state)
|
||||
planner.save_path(path, args.save_dir, args.n_save_state)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
meshTransformation()
|
||||
|
||||
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -2,15 +2,15 @@ import argparse
|
|||
|
||||
from usecases.stability_check_usecase import StabilityCheckUseCase
|
||||
|
||||
#python3 main.py --aspPath /home/idontsudo/t/framework/asp/out/sdf-generation --buildNumber 3
|
||||
# python3 main.py --aspPath /home/idontsudo/t/framework/asp/out/sdf-generation --buildNumber 3
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--aspPath', help='asp folder generation path')
|
||||
parser.add_argument('--buildNumber', help='FreeCad generation buildNumber')
|
||||
args = parser.parse_args()
|
||||
# args.aspPath
|
||||
# args.buildNumber
|
||||
StabilityCheckUseCase().call( args.aspPath,args.buildNumber )
|
||||
StabilityCheckUseCase().call(args.aspPath, args.buildNumber)
|
||||
|
||||
|
||||
main()
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ class StabilityCheckUseCase:
|
|||
def call(self, outPath: str, buildNumber: int, duration=500):
|
||||
DURATION = duration
|
||||
try:
|
||||
# Получение сгенерированного URDF для сборки
|
||||
assemblyUrdf = json.loads(
|
||||
(open(outPath + 'urdf-generation.json')).read()).get(buildNumber)
|
||||
except:
|
||||
|
|
@ -41,6 +42,7 @@ class StabilityCheckUseCase:
|
|||
inc = 0
|
||||
for el in bulletIds:
|
||||
inc += 1
|
||||
# Получение расположения деталей
|
||||
pos, rot = p.getBasePositionAndOrientation(el)
|
||||
resultCoords.append({
|
||||
'id': inc,
|
||||
|
|
@ -52,8 +54,8 @@ class StabilityCheckUseCase:
|
|||
|
||||
file_to_open = outPath + buildNumber + "_" + 'stability_coords.json'
|
||||
|
||||
f = open(file_to_open, 'w', encoding='utf-8',
|
||||
errors='ignore')
|
||||
f = open(file_to_open, 'w', encoding='utf-8',errors='ignore')
|
||||
# Запись результатов тестирования
|
||||
f.write(json.dumps(resultCoords))
|
||||
for el in urdfs:
|
||||
os.remove(el)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue