diff --git a/asp/.gitignore b/asp/.gitignore new file mode 100644 index 0000000..c585e19 --- /dev/null +++ b/asp/.gitignore @@ -0,0 +1 @@ +out \ No newline at end of file diff --git a/asp/main.py b/asp/main.py index da89e06..48240cd 100644 --- a/asp/main.py +++ b/asp/main.py @@ -1,17 +1,14 @@ import argparse import shutil from helper.fs import FS -from src.usecases.stability_check_usecase import StabilityCheckUseCase from src.usecases.urdf_sub_assembly_usecase import UrdfSubAssemblyUseCase -from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase +# from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase from src.model.sdf_geometry import GeometryModel from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase import os -# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true --format 'urdf' --stabilityCheck 'true' -# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true --format 'sdf' if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -19,8 +16,6 @@ if __name__ == "__main__": parser.add_argument('--outPath', help='save SDF path') parser.add_argument('--world', help='adding sdf world') parser.add_argument('--format', help='urdf,sdf,mujoco') - parser.add_argument('--stabilityCheck', - help='do i need to check the stability?') args = parser.parse_args() if args.generationFolder == None or args.outPath == None: @@ -44,13 +39,10 @@ if __name__ == "__main__": generationFolder=args.generationFolder, outPath=args.outPath ) - if (args.format == 'urdf' and args.stabilityCheck != None): + if (args.format == 'urdf'): UrdfSubAssemblyUseCase().call( geometryModels=geometryModels, assembly=assemblyStructure, world=args.world, generationFolder=args.generationFolder, outPath=args.outPath ) - StabilityCheckUseCase().call( - args.outPath - ) diff --git a/asp/src/usecases/formatter_usecase.py b/asp/src/usecases/formatter_usecase.py index fa0de3f..9efa2e8 100644 --- a/asp/src/usecases/formatter_usecase.py +++ b/asp/src/usecases/formatter_usecase.py @@ -1,11 +1,10 @@ from src.model.enum import Enum -import xmlformatter from helper.fs import FS - +import xmlformatter class FormatterUseCase: def call(outPath: str, format: str): - formatter = xmlformatter.Formatter( + formatter = xmlformatter( indent="1", indent_char="\t", encoding_output="ISO-8859-1", preserve=["literal"]) files = FS.readFilesTypeFolder( diff --git a/asp/src/usecases/stability_check_usecase.py b/asp/src/usecases/stability_check_usecase.py deleted file mode 100644 index c55b092..0000000 --- a/asp/src/usecases/stability_check_usecase.py +++ /dev/null @@ -1,36 +0,0 @@ -import numpy as np -import pybullet as p -import time -import pybullet_data -from helper.fs import FS -from src.usecases.urdf_sub_assembly_usecase import URDF_GENERATOR_FILE -import json - -from src.model.enum import Enum - - -class StabilityCheckUseCase: - def call(self, outPath: str): - dirPath = outPath + Enum.folderPath - DURATION = 10000 - asm = json.loads(FS.readFile(dirPath + URDF_GENERATOR_FILE)) - inc = 0 - for el in asm['asm2']: - FS.writeFile(data=el, filePath=dirPath, - fileName=str(inc) + '.urdf') - inc += 1 - assemblyURDFS = list( - map(lambda el: dirPath+el, FS.readFilesTypeFolder(dirPath, '.urdf'))) - physicsClient = p.connect(p.GUI) - - p.setGravity(0, 0, -10) - - for el in assemblyURDFS: - p.loadURDF(el) - - - for i in range(DURATION): - p.stepSimulation() - time.sleep(1./240.) - - p.disconnect() diff --git a/asp/src/usecases/urdf_sub_assembly_usecase.py b/asp/src/usecases/urdf_sub_assembly_usecase.py index 0c82827..907662e 100644 --- a/asp/src/usecases/urdf_sub_assembly_usecase.py +++ b/asp/src/usecases/urdf_sub_assembly_usecase.py @@ -5,6 +5,7 @@ from src.model.asm import Assembly from src.model.sdf_geometry import GeometryModel from helper.fs import filterModels, listGetFirstValue import json +import re def toUrdf(el: GeometryModel): @@ -21,6 +22,7 @@ class UrdfSubAssemblyUseCase(Assembly): generateSubAssemblyModels = self.generateSubAssembly(assembly) inc = 0 for key, value in generateSubAssemblyModels.items(): + keyAsm = int(re.findall(r'\d', key)[0]) inc += 1 if value['assembly'].__len__() != 0: model: Optional[GeometryModel] = listGetFirstValue( @@ -32,11 +34,9 @@ class UrdfSubAssemblyUseCase(Assembly): geometryModels, value['assembly']))) urdfs.append(listGetFirstValue( geometryModels, None, lambda x: x.name == value['part']) .toUrdf()) - asm[key] = urdfs + asm[keyAsm] = urdfs self.copy(generationFolder=generationFolder, format='/sdf', outPath=outPath) - FS.writeFile(data=json.dumps(asm), fileName=URDF_GENERATOR_FILE, filePath=dirPath) - # for el in asm.keys(): diff --git a/cad_generation/.gitignore b/cad_generation/.gitignore deleted file mode 100644 index 3e9f3c7..0000000 --- a/cad_generation/.gitignore +++ /dev/null @@ -1 +0,0 @@ -env.json \ No newline at end of file diff --git a/cad_generation/env.json b/cad_generation/env.json new file mode 100644 index 0000000..3945e7a --- /dev/null +++ b/cad_generation/env.json @@ -0,0 +1,6 @@ +{ + "doc": "/home/idontsudo/t/framework/asp-review-app/server/public/cubes/cubes.FCStd", + "out": "/home/idontsudo/t/framework/cad_generation", + "resultURL": "http://localhost:3002/assembly/save/out", + "projectId": "cubes" +} \ No newline at end of file diff --git a/cad_generation/helper/fs.py b/cad_generation/helper/fs.py index 9068e9a..3026bb4 100644 --- a/cad_generation/helper/fs.py +++ b/cad_generation/helper/fs.py @@ -9,6 +9,7 @@ class FS: def writeFile(data, filePath, fileName): file_to_open = filePath + fileName - f = open(file_to_open, 'w', ) - + f = open(file_to_open, 'w', encoding='utf-8', + errors='ignore') f.write(data) + f.close() diff --git a/cad_generation/main.py b/cad_generation/main.py index c6af337..83ddba7 100644 --- a/cad_generation/main.py +++ b/cad_generation/main.py @@ -11,8 +11,8 @@ def main(): env = FS.readJSON('./env.json') App.openDocument(env.get('doc')) RobossemblerFreeCadExportScenario().call(env.get('out')) - requests.post(url=env.get('resultURL'), files={'zip': open(env.get('out') + '/' + 'generation.zip', "rb"), 'id':env.get('projectId')}) - os.remove('./generation.zip') + # requests.post(url=env.get('resultURL'), files={'zip': open(env.get('out') + '/' + 'generation.zip', "rb"), 'id':env.get('projectId')}) + # os.remove('./generation.zip') App.closeDocument(App.ActiveDocument.Name) freecadQTWindow = Gui.getMainWindow() freecadQTWindow.close() diff --git a/cad_generation/scenarios/robossembler_freecad_export_scenario.py b/cad_generation/scenarios/robossembler_freecad_export_scenario.py index 4c15b6b..c834efb 100644 --- a/cad_generation/scenarios/robossembler_freecad_export_scenario.py +++ b/cad_generation/scenarios/robossembler_freecad_export_scenario.py @@ -46,9 +46,7 @@ class RobossemblerFreeCadExportScenario: def geometry(self, outPutsPath: str): exportUseCase = ExportUseCase.call(outPutsPath,EXPORT_TYPES.OBJ) for el in SdfGeometryUseCase().call(exportUseCase): - FS.writeFile(el.toJSON(), outPutsPath + - '/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',) - + FS.writeFile(el.toJSON(), outPutsPath + '/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',) diff --git a/cad_generation/usecases/get_sdf_geometry_usecase.py b/cad_generation/usecases/get_sdf_geometry_usecase.py index 2516164..45869c5 100644 --- a/cad_generation/usecases/get_sdf_geometry_usecase.py +++ b/cad_generation/usecases/get_sdf_geometry_usecase.py @@ -18,20 +18,6 @@ class SdfGeometryUseCase: try: for el in App.ActiveDocument.Objects: if is_object_solid(el): - for prop in self.PartPropertyCheck: - if prop in el: - App.Console.PrintMessage(el.Label + ' ' + 'Dont exists property: ' + prop) - return - for prop in self.ShapePropertyCheck: - if prop in el.Shape: - App.Console.PrintMessage(el.Label + ' ' + 'Dont exists property: ' + prop) - return - # com = el.Shape.CenterOfMass or el.Shape.CenterOfGravity - # if "Shape" in el: - # App.Console.PrintMessage(el.Label + ' ' + 'Dont exists Shape') - # return - # if "Mass" in el.Shape: - mass = el.Shape.Mass inertia = el.Shape.MatrixOfInertia pos = el.Shape.Placement @@ -73,6 +59,7 @@ class SdfGeometryUseCase: ) except Exception as e: print(200) + return geometry diff --git a/cad_stability_check/.gitignore b/cad_stability_check/.gitignore new file mode 100644 index 0000000..2b2f40c --- /dev/null +++ b/cad_stability_check/.gitignore @@ -0,0 +1,95 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*,cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# dotenv +.env + +# virtualenv +.venv +venv/ +ENV/ + +# Spyder project settings +.spyderproject + +# Rope project settings +.ropeproject +env.json \ No newline at end of file diff --git a/cad_stability_check/main.py b/cad_stability_check/main.py new file mode 100644 index 0000000..b4684e8 --- /dev/null +++ b/cad_stability_check/main.py @@ -0,0 +1,79 @@ +import FreeCAD as App +import json +import re +from pyquaternion import Quaternion + +def importObjAtPath(path: str): + import importOBJ + importOBJ.insert(u"" + path, App.ActiveDocument.Label) + + pass + + +def getFullPathObj(assemblyFolder: str, name: str): + return assemblyFolder + 'sdf/meshes/' + name + '.obj' + + +def computedStabiliti(refElement, childElement): + b = childElement.Shape.BoundBox + App.activeDocument().addObject("Part::MultiCommon", "Common") + App.activeDocument().Common.Shapes = [refElement, childElement, ] + App.ActiveDocument.getObject('Common').ViewObject.ShapeColor = getattr(App.getDocument('cubes').getObject( + refElement.Name).getLinkedObject(True).ViewObject, 'ShapeColor', App.getDocument('cubes').getObject('Common').ViewObject.ShapeColor) + App.ActiveDocument.getObject('Common').ViewObject.DisplayMode = getattr(App.getDocument('cubes').getObject( + childElement.Name).getLinkedObject(True).ViewObject, 'DisplayMode', App.getDocument('cubes').getObject('Common').ViewObject.DisplayMode) + App.ActiveDocument.recompute() + obj = App.ActiveDocument.getObjectsByLabel('Common')[0] + + shp = obj.Shape + bbox = shp.BoundBox + if bbox.XLength == b.XLength and bbox.YLength == b.YLength and b.ZLength == bbox.ZLength: + return True + return False + + +def main(): + App.newDocument() + env = json.loads((open('./env.json')).read()) + coordinatsFilePath = env.get('pathToTheSimulationCoordinatesFile') + assemblyFolder = env.get('generationFolder') + buildNumber = int(re.findall(r'\d', coordinatsFilePath)[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)) + importObjAtPath(getFullPathObj( + assemblyFolder, detailOfTheMarkingZoneOfWhich)) + meshMark = App.ActiveDocument.Objects[0] + meshDetailOfTheMarkZone = App.ActiveDocument.Objects[1] + meshMark.ViewObject.ShapeColor = (0.31, 0.77, 0.87) + meshDetailOfTheMarkZone.ViewObject.ShapeColor = (0.68, 0.66, 0.95) + for el in list(map(lambda el: getFullPathObj(assemblyFolder, el), subassemblyNotParticipatingInMarkup)): + 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()) + inc = 1 + for el in App.ActiveDocument.Objects: + pos = coordinats[inc]['position'] + qua = coordinats[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_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)) + pass + + +main() diff --git a/pddl/main.py b/pddl/main.py index 8541124..6766cce 100644 --- a/pddl/main.py +++ b/pddl/main.py @@ -5,7 +5,7 @@ from src.model.asm4_structure import Asm4Structure from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase -# python3 main.py --stepStructurePath /Users/idontsudo/robo/Cube3/step-structure.json --outPath /Users/idontsudo/robo/Cube3/pddl/ +# python3 main.py --stepStructurePath /home/idontsudo/t/framework/asp-review-app/server/public/cubes/generation/step-structure.json --outPath /home/idontsudo/t/framework/pddl/ if __name__ == "__main__": parser = argparse.ArgumentParser() diff --git a/stability_check_predicate/main.py b/stability_check_predicate/main.py deleted file mode 100644 index a4e0304..0000000 --- a/stability_check_predicate/main.py +++ /dev/null @@ -1,16 +0,0 @@ -import argparse - -from usecases.stability_check_usecase import StabilityCheckUseCase - -#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 ) - - -main() \ No newline at end of file diff --git a/stability_check_predicate/usecases/stability_check_usecase.py b/stability_check_predicate/usecases/stability_check_usecase.py deleted file mode 100644 index 9d37b53..0000000 --- a/stability_check_predicate/usecases/stability_check_usecase.py +++ /dev/null @@ -1,61 +0,0 @@ -import numpy as np -import pybullet as p -import time -import pybullet_data -import os -import json - - -class StabilityCheckUseCase: - def call(self, outPath: str, buildNumber: int, duration=500): - DURATION = duration - try: - assemblyUrdf = json.loads( - (open(outPath + 'urdf-generation.json')).read()).get(buildNumber) - except: - return TypeError('not found urfd file or not found build number') - inc = 0 - urdfs = [] - - for el in assemblyUrdf: - inc += 1 - file_to_open = outPath + str(inc) + '.urdf' - - f = open(file_to_open, 'w', encoding='utf-8', - errors='ignore') - f.write(el) - urdfs.append(os.path.abspath(f.name)) - f.close() - - p.connect(p.DIRECT) - - p.setGravity(0, 0, -10) - p.setAdditionalSearchPath(pybullet_data.getDataPath()) - bulletIds = [] - for el in urdfs: - bulletIds.append(p.loadURDF(el)) - p.loadURDF("plane.urdf") - resultCoords = [] - for i in range(DURATION): - if (i + 200 == DURATION): - inc = 0 - for el in bulletIds: - inc += 1 - pos, rot = p.getBasePositionAndOrientation(el) - resultCoords.append({ - 'id': inc, - "quaternion": rot, - "position": pos - }) - p.stepSimulation() - time.sleep(1./240.) - - file_to_open = outPath + buildNumber + "_" + 'stability_coords.json' - - f = open(file_to_open, 'w', encoding='utf-8', - errors='ignore') - f.write(json.dumps(resultCoords)) - for el in urdfs: - os.remove(el) - f.close() - p.disconnect()