From a38c3bec5abfebed71289c7006de06a5a99a84c1 Mon Sep 17 00:00:00 2001 From: IDONTSUDO Date: Sun, 18 Jun 2023 15:33:16 +0000 Subject: [PATCH] Support "Assemble them all" trajectory generation from CAD --- .gitmodules | 3 + {sdf => asp}/helper/fs.py | 0 {sdf => asp}/main.py | 0 {sdf => asp}/mocks/Cube1.json | 0 {sdf => asp}/mocks/Cube2.json | 0 {sdf => asp}/mocks/sdf/include.sdf | 0 {sdf => asp}/mocks/sdf/include_pose.sdf | 0 {sdf => asp}/mocks/sdf/joint_fixed.sdf | 0 {sdf => asp}/mocks/sdf/link.sdf | 0 {sdf => asp}/mocks/sdf/model.config | 0 {sdf => asp}/mocks/sdf/model.sdf | 0 {sdf => asp}/mocks/sdf/world.sdf | 0 {sdf => asp}/mocks/urdf/model.urdf | 0 asp/requirements.txt | 5 + {sdf => asp}/src/model/asm.py | 0 {sdf => asp}/src/model/enum.py | 0 {sdf => asp}/src/model/sdf_geometry.py | 0 {sdf => asp}/src/model/sdf_join.py | 0 .../src/usecases/formatter_usecase.py | 0 {sdf => asp}/src/usecases/generate_world.py | 0 .../usecases/sdf_generate_world_usecase.py | 0 .../src/usecases/sdf_sub_assembly_usecase.py | 0 .../src/usecases/stability_check_usecase.py | 0 .../src/usecases/urdf_sub_assembly_usecase.py | 0 cg/freecad/Frames/Frames.py | 6 +- cg/freecad/Frames/InitGui.py | 3 +- cg/freecad/Frames/Tools.py | 7 ++ .../Frames/model/connected_part_model.py | 34 +++++ cg/freecad/Frames/model/files_generator.py | 1 + cg/freecad/Frames/model/join_mesh_model.py | 33 +++++ cg/freecad/Frames/model/mesh_part_model.py | 32 +++++ .../Frames/model/simple_copy_part_model.py | 31 +++++ .../robossembler_freecad_export_scenario.py | 20 +-- .../Frames/usecases/assembly_parse_usecase.py | 40 ++++-- .../export_assembly_them_all_usecase.py | 92 ++++++++++++++ cg/freecad/Frames/usecases/export_usecase.py | 34 +++-- .../Frames/usecases/geometry_usecase.py | 90 ++++++------- .../usecases/get_sdf_geometry_usecase.py | 102 ++++++++------- insertion_vector_predicate/assembly | 1 + insertion_vector_predicate/main.py | 119 ++++++++++++++++++ insertion_vector_predicate/requirements.txt | 2 + sdf/requirements.txt | 1 - 42 files changed, 537 insertions(+), 119 deletions(-) create mode 100644 .gitmodules rename {sdf => asp}/helper/fs.py (100%) rename {sdf => asp}/main.py (100%) rename {sdf => asp}/mocks/Cube1.json (100%) rename {sdf => asp}/mocks/Cube2.json (100%) rename {sdf => asp}/mocks/sdf/include.sdf (100%) rename {sdf => asp}/mocks/sdf/include_pose.sdf (100%) rename {sdf => asp}/mocks/sdf/joint_fixed.sdf (100%) rename {sdf => asp}/mocks/sdf/link.sdf (100%) rename {sdf => asp}/mocks/sdf/model.config (100%) rename {sdf => asp}/mocks/sdf/model.sdf (100%) rename {sdf => asp}/mocks/sdf/world.sdf (100%) rename {sdf => asp}/mocks/urdf/model.urdf (100%) create mode 100644 asp/requirements.txt rename {sdf => asp}/src/model/asm.py (100%) rename {sdf => asp}/src/model/enum.py (100%) rename {sdf => asp}/src/model/sdf_geometry.py (100%) rename {sdf => asp}/src/model/sdf_join.py (100%) rename {sdf => asp}/src/usecases/formatter_usecase.py (100%) rename {sdf => asp}/src/usecases/generate_world.py (100%) rename {sdf => asp}/src/usecases/sdf_generate_world_usecase.py (100%) rename {sdf => asp}/src/usecases/sdf_sub_assembly_usecase.py (100%) rename {sdf => asp}/src/usecases/stability_check_usecase.py (100%) rename {sdf => asp}/src/usecases/urdf_sub_assembly_usecase.py (100%) create mode 100644 cg/freecad/Frames/model/connected_part_model.py create mode 100644 cg/freecad/Frames/model/join_mesh_model.py create mode 100644 cg/freecad/Frames/model/mesh_part_model.py create mode 100644 cg/freecad/Frames/model/simple_copy_part_model.py create mode 100644 cg/freecad/Frames/usecases/export_assembly_them_all_usecase.py create mode 160000 insertion_vector_predicate/assembly create mode 100644 insertion_vector_predicate/main.py create mode 100644 insertion_vector_predicate/requirements.txt delete mode 100644 sdf/requirements.txt diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..8ec0906 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "insertion_vector_predicate/assembly"] + path = insertion_vector_predicate/assembly + url = https://github.com/yunshengtian/Assemble-Them-All diff --git a/sdf/helper/fs.py b/asp/helper/fs.py similarity index 100% rename from sdf/helper/fs.py rename to asp/helper/fs.py diff --git a/sdf/main.py b/asp/main.py similarity index 100% rename from sdf/main.py rename to asp/main.py diff --git a/sdf/mocks/Cube1.json b/asp/mocks/Cube1.json similarity index 100% rename from sdf/mocks/Cube1.json rename to asp/mocks/Cube1.json diff --git a/sdf/mocks/Cube2.json b/asp/mocks/Cube2.json similarity index 100% rename from sdf/mocks/Cube2.json rename to asp/mocks/Cube2.json diff --git a/sdf/mocks/sdf/include.sdf b/asp/mocks/sdf/include.sdf similarity index 100% rename from sdf/mocks/sdf/include.sdf rename to asp/mocks/sdf/include.sdf diff --git a/sdf/mocks/sdf/include_pose.sdf b/asp/mocks/sdf/include_pose.sdf similarity index 100% rename from sdf/mocks/sdf/include_pose.sdf rename to asp/mocks/sdf/include_pose.sdf diff --git a/sdf/mocks/sdf/joint_fixed.sdf b/asp/mocks/sdf/joint_fixed.sdf similarity index 100% rename from sdf/mocks/sdf/joint_fixed.sdf rename to asp/mocks/sdf/joint_fixed.sdf diff --git a/sdf/mocks/sdf/link.sdf b/asp/mocks/sdf/link.sdf similarity index 100% rename from sdf/mocks/sdf/link.sdf rename to asp/mocks/sdf/link.sdf diff --git a/sdf/mocks/sdf/model.config b/asp/mocks/sdf/model.config similarity index 100% rename from sdf/mocks/sdf/model.config rename to asp/mocks/sdf/model.config diff --git a/sdf/mocks/sdf/model.sdf b/asp/mocks/sdf/model.sdf similarity index 100% rename from sdf/mocks/sdf/model.sdf rename to asp/mocks/sdf/model.sdf diff --git a/sdf/mocks/sdf/world.sdf b/asp/mocks/sdf/world.sdf similarity index 100% rename from sdf/mocks/sdf/world.sdf rename to asp/mocks/sdf/world.sdf diff --git a/sdf/mocks/urdf/model.urdf b/asp/mocks/urdf/model.urdf similarity index 100% rename from sdf/mocks/urdf/model.urdf rename to asp/mocks/urdf/model.urdf diff --git a/asp/requirements.txt b/asp/requirements.txt new file mode 100644 index 0000000..2a50401 --- /dev/null +++ b/asp/requirements.txt @@ -0,0 +1,5 @@ +argparse +matplotlib +pybullet +argparse +xmlformatter \ No newline at end of file diff --git a/sdf/src/model/asm.py b/asp/src/model/asm.py similarity index 100% rename from sdf/src/model/asm.py rename to asp/src/model/asm.py diff --git a/sdf/src/model/enum.py b/asp/src/model/enum.py similarity index 100% rename from sdf/src/model/enum.py rename to asp/src/model/enum.py diff --git a/sdf/src/model/sdf_geometry.py b/asp/src/model/sdf_geometry.py similarity index 100% rename from sdf/src/model/sdf_geometry.py rename to asp/src/model/sdf_geometry.py diff --git a/sdf/src/model/sdf_join.py b/asp/src/model/sdf_join.py similarity index 100% rename from sdf/src/model/sdf_join.py rename to asp/src/model/sdf_join.py diff --git a/sdf/src/usecases/formatter_usecase.py b/asp/src/usecases/formatter_usecase.py similarity index 100% rename from sdf/src/usecases/formatter_usecase.py rename to asp/src/usecases/formatter_usecase.py diff --git a/sdf/src/usecases/generate_world.py b/asp/src/usecases/generate_world.py similarity index 100% rename from sdf/src/usecases/generate_world.py rename to asp/src/usecases/generate_world.py diff --git a/sdf/src/usecases/sdf_generate_world_usecase.py b/asp/src/usecases/sdf_generate_world_usecase.py similarity index 100% rename from sdf/src/usecases/sdf_generate_world_usecase.py rename to asp/src/usecases/sdf_generate_world_usecase.py diff --git a/sdf/src/usecases/sdf_sub_assembly_usecase.py b/asp/src/usecases/sdf_sub_assembly_usecase.py similarity index 100% rename from sdf/src/usecases/sdf_sub_assembly_usecase.py rename to asp/src/usecases/sdf_sub_assembly_usecase.py diff --git a/sdf/src/usecases/stability_check_usecase.py b/asp/src/usecases/stability_check_usecase.py similarity index 100% rename from sdf/src/usecases/stability_check_usecase.py rename to asp/src/usecases/stability_check_usecase.py diff --git a/sdf/src/usecases/urdf_sub_assembly_usecase.py b/asp/src/usecases/urdf_sub_assembly_usecase.py similarity index 100% rename from sdf/src/usecases/urdf_sub_assembly_usecase.py rename to asp/src/usecases/urdf_sub_assembly_usecase.py diff --git a/cg/freecad/Frames/Frames.py b/cg/freecad/Frames/Frames.py index ea46662..8fd823b 100644 --- a/cg/freecad/Frames/Frames.py +++ b/cg/freecad/Frames/Frames.py @@ -312,11 +312,7 @@ Tools.spawnClassCommand("FrameCommand", "MenuText": "Make a free frame", "ToolTip": "Make a freestanding reference frame."}) -Tools.spawnClassCommand("ASM4StructureParsing", - RobossemblerFreeCadExportScenario().call, - {"Pixmap": str(os.path.join(icondir, "assembly4.svg")), - "MenuText": "Make a ASM4 parsing", - "ToolTip": "Make a ASM4 1"}) + Tools.spawnClassCommand("SelectedPartFrameCommand", makeSelectedPartFrames, {"Pixmap": str(os.path.join(icondir, "partframe.svg")), diff --git a/cg/freecad/Frames/InitGui.py b/cg/freecad/Frames/InitGui.py index 536ec0a..2bbad93 100644 --- a/cg/freecad/Frames/InitGui.py +++ b/cg/freecad/Frames/InitGui.py @@ -44,7 +44,8 @@ class Frames(Workbench): self.toolcommands = [ "ExportPlacementAndPropertiesCommand", "ExportGazeboModels", - "InsertGraspPose" + "InsertGraspPose", + "ASM4StructureParsing" ] self.appendToolbar(f"{__class__.__name__} Frames", self.framecommands) self.appendToolbar(f"{__class__.__name__} Tools", self.toolcommands) diff --git a/cg/freecad/Frames/Tools.py b/cg/freecad/Frames/Tools.py index 484706d..cc6e9d2 100644 --- a/cg/freecad/Frames/Tools.py +++ b/cg/freecad/Frames/Tools.py @@ -17,6 +17,7 @@ import json # For exporting part infos import os # for safer path handling import GazeboExport import GraspPose +from scenarios.robossembler_freecad_export_scenario import RobossemblerFreeCadExportScenario if FreeCAD.GuiUp: import FreeCADGui from PySide import QtGui @@ -556,6 +557,12 @@ spawnClassCommand("InsertGraspPose", "MenuText": "Insert Grasp Pose", "ToolTip": "Insert Grasp Pose for Selected Part"}) +spawnClassCommand("ASM4StructureParsing", + RobossemblerFreeCadExportScenario().call, + {"Pixmap": str(os.path.join(icondir, "assembly4.svg")), + "MenuText": "Make a ASM4 parsing", + "ToolTip": "Make a ASM4 1"}) + ################################################################### # Information from primitive type diff --git a/cg/freecad/Frames/model/connected_part_model.py b/cg/freecad/Frames/model/connected_part_model.py new file mode 100644 index 0000000..9daef30 --- /dev/null +++ b/cg/freecad/Frames/model/connected_part_model.py @@ -0,0 +1,34 @@ +import BOPTools.JoinFeatures +import FreeCAD as App +import uuid + + +class ConnectedPartModel: + name = None + id = None + solid = None + + def __init__(self, part) -> None: + try: + self.id ='part' + str(uuid.uuid4()) + j = BOPTools.JoinFeatures.makeConnect(name=self.id) + if (type(part) is list): + j.Objects = part + else: + j.Objects = [part] + j.Proxy.execute(j) + j.purgeTouched() + self.solid = j + App.ActiveDocument.recompute() + except Exception as e: + print(e) + pass + + def remove(self): + try: + App.ActiveDocument.removeObject(self.solid.Label) + except Exception as e: + print(e) + + + \ No newline at end of file diff --git a/cg/freecad/Frames/model/files_generator.py b/cg/freecad/Frames/model/files_generator.py index 8b234b6..68b4a0b 100644 --- a/cg/freecad/Frames/model/files_generator.py +++ b/cg/freecad/Frames/model/files_generator.py @@ -10,3 +10,4 @@ class FolderGenerator(Enum): MESHES = 'meshes' ASSETS = 'assets' SDF = 'sdf' + ASSEMBlY = 'assembly' diff --git a/cg/freecad/Frames/model/join_mesh_model.py b/cg/freecad/Frames/model/join_mesh_model.py new file mode 100644 index 0000000..52ca96b --- /dev/null +++ b/cg/freecad/Frames/model/join_mesh_model.py @@ -0,0 +1,33 @@ +import FreeCAD +import Mesh +import FreeCAD as App +from model.mesh_part_model import MeshPartModel + + +class JoinMeshModel: + id = None + mesh = None + + def __init__(self, meshesPartModels: list['MeshPartModel']) -> None: + meshes = [] + import Mesh + from random import randrange + for el in meshesPartModels: + meshes.append(el.mesh.Mesh) + + self.id = 'MergedMesh' + str(randrange(1000000)) + document = App.ActiveDocument + merged_mesh = Mesh.Mesh() + for el in meshes: + merged_mesh.addMesh(el) + + new_obj = App.activeDocument().addObject("Mesh::Feature", self.id) + new_obj.Mesh = merged_mesh + new_obj.ViewObject.DisplayMode = "Flat Lines" # Set display mode to flat lines + self.mesh = new_obj + + def remove(self): + try: + App.ActiveDocument.removeObject(self.id) + except Exception as e: + print(e) diff --git a/cg/freecad/Frames/model/mesh_part_model.py b/cg/freecad/Frames/model/mesh_part_model.py new file mode 100644 index 0000000..1fab908 --- /dev/null +++ b/cg/freecad/Frames/model/mesh_part_model.py @@ -0,0 +1,32 @@ +import FreeCAD as App +import uuid +import Mesh +import Part +import PartGui +import MeshPart + + +class MeshPartModel: + id = None + mesh = None + + def __init__(self, part) -> None: + try: + from random import randrange + self.id = 'mesh' + str(randrange(1000000)) + document = App.ActiveDocument + mesh = document.addObject("Mesh::Feature", self.id) + shape = Part.getShape(part, "") + mesh.Mesh = MeshPart.meshFromShape( + Shape=shape, LinearDeflection=20, AngularDeflection=0.1, Relative=False) + mesh.Label = self.id + self.mesh = mesh + except Exception as e: + print(e) + pass + + def remove(self): + try: + App.ActiveDocument.removeObject(self.mesh.Label) + except Exception as e: + print(e) diff --git a/cg/freecad/Frames/model/simple_copy_part_model.py b/cg/freecad/Frames/model/simple_copy_part_model.py new file mode 100644 index 0000000..51f95d4 --- /dev/null +++ b/cg/freecad/Frames/model/simple_copy_part_model.py @@ -0,0 +1,31 @@ +import FreeCAD as App +import Part + + +class SimpleCopyPartModel: + id = None + copyLink = None + label = None + part = None + + def getPart(self): + return self.part + + def __init__(self, part) -> None: + try: + from random import randrange + self.id = str(randrange(1000000)) + childObj = part + print(part) + __shape = Part.getShape( + childObj, '', needSubElement=False, refine=False) + obj = App.ActiveDocument.addObject('Part::Feature', self.id) + obj.Shape = __shape + self.part = obj + self.label = obj.Label + App.ActiveDocument.recompute() + except Exception as e: + print(e) + + def remove(self): + App.ActiveDocument.removeObject(self.label) diff --git a/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py b/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py index 4868a93..71def3b 100644 --- a/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py +++ b/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py @@ -1,4 +1,7 @@ import FreeCAD +from usecases.export_assembly_them_all_usecase import ExportAssemblyThemAllUseCase + +from usecases.export_usecase import EXPORT_TYPES from usecases.export_usecase import ExportUseCase from usecases.get_sdf_geometry_usecase import SdfGeometryUseCase from usecases.assembly_parse_usecase import AssemblyParseUseCase @@ -41,16 +44,17 @@ class RobossemblerFreeCadExportScenario: os.makedirs(directory) __objs__ = FreeCAD.ActiveDocument.RootObjects - - os.makedirs(directory + '/' + FolderGenerator.ASSETS.value) - os.makedirs(directory + '/' + FolderGenerator.SDF.value) - os.makedirs(directory + '/' + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value) + directoryExport = directory + '/' + os.makedirs(directoryExport + FolderGenerator.ASSETS.value) + os.makedirs(directoryExport + FolderGenerator.SDF.value) + os.makedirs(directoryExport + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value) + os.makedirs(directoryExport + FolderGenerator.ASSEMBlY.value) f = open(directory + "/step-structure.json", "w") f.write(AssemblyParseUseCase().toJson()) f.close() self.geometry(directory) - - ImportGui.export(__objs__, directory + '/' + 'assembly.step') + ExportAssemblyThemAllUseCase().call(directoryExport) + ImportGui.export(__objs__, directoryExport + 'assembly.step') shutil.make_archive(directory, 'zip', directory) @@ -58,8 +62,8 @@ class RobossemblerFreeCadExportScenario: return True def geometry(self, outPutsPath: str): - meshesExportUseCase = ExportUseCase.call(outPutsPath) - for el in SdfGeometryUseCase.call(meshesExportUseCase): + exportUseCase = ExportUseCase.call(outPutsPath,EXPORT_TYPES.OBJ) + for el in SdfGeometryUseCase().call(exportUseCase): FS.writeFile(el.toJSON(), outPutsPath + '/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',) diff --git a/cg/freecad/Frames/usecases/assembly_parse_usecase.py b/cg/freecad/Frames/usecases/assembly_parse_usecase.py index b1cd291..e3b4e0d 100644 --- a/cg/freecad/Frames/usecases/assembly_parse_usecase.py +++ b/cg/freecad/Frames/usecases/assembly_parse_usecase.py @@ -1,20 +1,40 @@ import FreeCAD as App -from helper.is_solid import is_object_solid - +def is_object_solid(obj): + """If obj is solid return True""" + if not isinstance(obj, App.DocumentObject): + return False + if hasattr(obj, 'Group'): + return False + + if not hasattr(obj, 'Shape'): + return False + # if not hasattr(obj.Shape, 'Mass'): + # return False + if not hasattr(obj.Shape, 'Solids'): + return False + + if len(obj.Shape.Solids) == 0: + return False + + return True + class AssemblyParseUseCase: _parts = [] + _asm = [] + def getAsm(self): + return self._asm + def __init__(self) -> None: - self.initParse() + if (self._asm.__len__() == 0): + self.initParse() pass - - def initParse(self): - for el in App.ActiveDocument.Objects: - if(is_object_solid(el)): + for el in App.ActiveDocument.Objects: + if (is_object_solid(el)): self._asm.append(el.Label) def toJson(self): @@ -28,15 +48,11 @@ class AssemblyParseUseCase: if groupLink.get(el.Label) == None: groupLink[el.Label] = [] for i in el.Group: - if str(i).find('Pad') != -1: groupLink[el.Label].append(i) if groupLink.__len__() == 0: - return None + return None return groupLink def getLinkedProperty(self): return self._asm - - - diff --git a/cg/freecad/Frames/usecases/export_assembly_them_all_usecase.py b/cg/freecad/Frames/usecases/export_assembly_them_all_usecase.py new file mode 100644 index 0000000..f74c56f --- /dev/null +++ b/cg/freecad/Frames/usecases/export_assembly_them_all_usecase.py @@ -0,0 +1,92 @@ + + +from typing import List +import FreeCAD as App +import Part +from model.join_mesh_model import JoinMeshModel +from model.mesh_part_model import MeshPartModel +from helper.fs import FS +from helper.is_solid import is_object_solid +from model.simple_copy_part_model import SimpleCopyPartModel +from model.files_generator import FolderGenerator +from usecases.assembly_parse_usecase import AssemblyParseUseCase +import importOBJ +import os +import json + + +class ExportAssemblyThemAllUseCase: + + def call(self, path): + assembly = AssemblyParseUseCase().getAsm() + asmStructure = {} + inc = 0 + for el in assembly: + if (inc != 0): + asmStructure[inc] = { + "child": el, + "parents": assembly[0:inc] + } + inc += 1 + objectsFreeCad = App.ActiveDocument.Objects + asmSolids = {} + for k, v in asmStructure.items(): + assemblyParentList = v['parents'] + assemblyChild = v['child'] + for el in assemblyParentList: + for solid in objectsFreeCad: + if (el == solid.Label): + if (asmSolids.get(k) is None): + + asmSolids[k] = {'parents': [], 'child': list( + filter(lambda x: x.Label == assemblyChild, objectsFreeCad))[0]} + + asmSolids[k]['parents'].append(solid) + + inc = 0 + for k, v in asmSolids.items(): + geometry = {"0": [], "1": []} + if (k != 0): + App.activeDocument().addObject("Part::Compound", "Compound") + + copyLinks = list( + map(lambda el: SimpleCopyPartModel(el), v['parents'])) + + if copyLinks != None: + App.activeDocument().Compound.Links = list( + map(lambda el: el.getPart(), copyLinks)) + + object = App.activeDocument().getObject('Compound') + boundBox = object.Shape.BoundBox + geometry['0'].append(boundBox.XMax) + geometry['0'].append(boundBox.YMax) + geometry['0'].append(boundBox.ZMax) + + os.makedirs( + path + FolderGenerator.ASSEMBlY.value + '/' + '0000' + str(k)) + boundBoxChild = v['child'].Shape.BoundBox + geometry['1'].append(boundBoxChild.XMax) + geometry['1'].append(boundBoxChild.YMax) + geometry['1'].append(boundBoxChild.ZMax) + meshParents = [] + + for el in v['parents']: + meshParents.append(MeshPartModel(el)) + joinMesh = JoinMeshModel(meshParents) + for el in meshParents: + el.remove() + importOBJ.export(joinMesh.mesh, path + FolderGenerator.ASSEMBlY.value + + '/' + '0000' + str(k) + '/' + str(1) + '.obj') + joinMesh.remove() + importOBJ.export(v['child'], path + FolderGenerator.ASSEMBlY.value + + '/' + '0000' + str(k) + '/' + str(0) + '.obj') + FS.writeFile(json.dumps(geometry), path + FolderGenerator.ASSEMBlY.value + + '/' + '0000' + str(k) + '/', 'translation.json') + + App.ActiveDocument.removeObject("Compound") + for el in copyLinks: + el.remove() + App.activeDocument().recompute() + inc += 1 + + diff --git a/cg/freecad/Frames/usecases/export_usecase.py b/cg/freecad/Frames/usecases/export_usecase.py index bce09c6..c555bc1 100644 --- a/cg/freecad/Frames/usecases/export_usecase.py +++ b/cg/freecad/Frames/usecases/export_usecase.py @@ -1,16 +1,36 @@ -# import importDAE +import importDAE +import importOBJ +import Mesh import FreeCAD as App from model.files_generator import FolderGenerator from helper.is_solid import is_object_solid -import Mesh +from enum import Enum + +class EXPORT_TYPES(Enum): + STL = 'STL' + DAO = 'DAO' + OBJ = 'OBJ' + class ExportUseCase: - def call(path): + def call(path: str, type: EXPORT_TYPES): meshes = {} for el in App.ActiveDocument.Objects: if (is_object_solid(el)): - Mesh.export([el], path + '/' + FolderGenerator.SDF.value + - '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae') - meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \ - '/' + el.Label + '.dae' + match type.value: + case EXPORT_TYPES.STL.value: + Mesh.export([el], path + '/' + FolderGenerator.SDF.value + + '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.stl') + meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \ + '/' + el.Label + '.stl' + + case EXPORT_TYPES.DAO.value: + importDAE.export([el], path + '/' + FolderGenerator.SDF.value + + '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae') + case EXPORT_TYPES.OBJ.value: + importOBJ.export([el], path + '/' + FolderGenerator.SDF.value + + '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.obj') + meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \ + '/' + el.Label + '.obj' + return meshes diff --git a/cg/freecad/Frames/usecases/geometry_usecase.py b/cg/freecad/Frames/usecases/geometry_usecase.py index d4df666..13ebfd2 100644 --- a/cg/freecad/Frames/usecases/geometry_usecase.py +++ b/cg/freecad/Frames/usecases/geometry_usecase.py @@ -6,49 +6,53 @@ from helper.is_solid import is_object_solid class GeometryUseCase: def call() -> dict: labels = [] + Error = False for el in App.ActiveDocument.Objects: - - if is_object_solid(el): - labels.append(el.Label) - - geometry = { - "euler": { - "x": None, - "y": None, - "z": None - }, - "position": { - "x": None, - "y": None, - "z": None - }, - "rotation": { - "x": None, - "y": None, - "z": None - }, - "center": { - "x": None, - "y": None, - "z": None - }, - - } - - boundBox = el.Shape.BoundBox - geometry["center"]["x"] = boundBox.Center.x - geometry["center"]["y"] = boundBox.Center.y - geometry["center"]["z"] = boundBox.Center.z - geometry["position"]['x'] = boundBox.XMax - geometry["position"]['y'] = boundBox.YMax - geometry["position"]['z'] = boundBox.ZMax - rotation = el.Placement.Rotation - geometry["rotation"]['x'] = rotation.Axis.z - geometry["rotation"]['y'] = rotation.Axis.y - geometry["rotation"]['z'] = rotation.Axis.z - euler = el.Placement.Rotation.toEuler() - geometry["euler"]['x'] = euler[0] - geometry["euler"]['y'] = euler[1] - geometry["euler"]['z'] = euler[2] + try: + + if is_object_solid(el): + labels.append(el.Label) + + geometry = { + "euler": { + "x": None, + "y": None, + "z": None + }, + "position": { + "x": None, + "y": None, + "z": None + }, + "rotation": { + "x": None, + "y": None, + "z": None + }, + "center": { + "x": None, + "y": None, + "z": None + }, + + } + boundBox = el.Shape.BoundBox + geometry["center"]["x"] = boundBox.Center.x + geometry["center"]["y"] = boundBox.Center.y + geometry["center"]["z"] = boundBox.Center.z + geometry["position"]['x'] = boundBox.XMax + geometry["position"]['y'] = boundBox.YMax + geometry["position"]['z'] = boundBox.ZMax + rotation = el.Placement.Rotation + geometry["rotation"]['x'] = rotation.Axis.z + geometry["rotation"]['y'] = rotation.Axis.y + geometry["rotation"]['z'] = rotation.Axis.z + euler = el.Placement.Rotation.toEuler() + geometry["euler"]['x'] = euler[0] + geometry["euler"]['y'] = euler[1] + geometry["euler"]['z'] = euler[2] + except Exception as e: + print(e) + # App.Console.PrintMessage("Clicked on position: ("+str(pos[0])+", "+str(pos[1])+")\n") return {"geometry": geometry, "labels": labels, "label": el.Label} diff --git a/cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py b/cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py index d3c6ee3..2516164 100644 --- a/cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py +++ b/cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py @@ -5,7 +5,9 @@ from helper.is_solid import is_object_solid class SdfGeometryUseCase: - def call(stlPaths:dict) -> list[SdfGeometryModel]: + ShapePropertyCheck = ['Mass','MatrixOfInertia','Placement', ] + PartPropertyCheck = ['Shape'] + def call(self, stlPaths:dict) -> list[SdfGeometryModel]: materialSolid = {} for el in App.ActiveDocument.Objects: if str(el) == '': @@ -13,48 +15,64 @@ class SdfGeometryUseCase: for i in el.References: materialSolid[i[0].Label] = friction geometry = [] - for el in App.ActiveDocument.Objects: - if is_object_solid(el): - com = el.Shape.CenterOfMass - mass = el.Shape.Mass - inertia = el.Shape.MatrixOfInertia - pos = el.Shape.Placement - inertia = el.Shape.MatrixOfInertia - name = el.Label - ixx = str(inertia.A11 / 1000000) - ixy = str(inertia.A12 / 1000000) - ixz = str(inertia.A13 / 1000000) - iyy = str(inertia.A22 / 1000000) - iyz = str(inertia.A23 / 1000000) - izz = str(inertia.A33 / 1000000) - massSDF = str(mass / 1000000) - posX = str(pos.Base[0] / 1000000) - posY = str(pos.Base[1] / 1000000) - posZ = str(pos.Base[2] / 1000000) - eulerX = str(pos.Rotation.toEuler()[0]) - eulerY = str(pos.Rotation.toEuler()[1]) - eulerZ = str(pos.Rotation.toEuler()[2]) - - geometry.append( - SdfGeometryModel( - stl=stlPaths.get(el.Label), - name=name, - ixx=ixx, - ixz=ixz, - ixy=ixy, - iyy=iyy, - iyz=iyz, - izz=izz, - massSDF=massSDF, - posX=posX, - posY=posY, - posZ=posZ, - eulerX=eulerX, - eulerY=eulerY, - eulerZ=eulerZ, - friction=materialSolid.get(el.Label) or '', + 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 + inertia = el.Shape.MatrixOfInertia + name = el.Label + ixx = str(inertia.A11 / 1000000) + ixy = str(inertia.A12 / 1000000) + ixz = str(inertia.A13 / 1000000) + iyy = str(inertia.A22 / 1000000) + iyz = str(inertia.A23 / 1000000) + izz = str(inertia.A33 / 1000000) + massSDF = str(mass / 1000000) + posX = str(pos.Base[0] / 1000000) + posY = str(pos.Base[1] / 1000000) + posZ = str(pos.Base[2] / 1000000) + eulerX = str(pos.Rotation.toEuler()[0]) + eulerY = str(pos.Rotation.toEuler()[1]) + eulerZ = str(pos.Rotation.toEuler()[2]) + + geometry.append( + SdfGeometryModel( + stl=stlPaths.get(el.Label), + name=name, + ixx=ixx, + ixz=ixz, + ixy=ixy, + iyy=iyy, + iyz=iyz, + izz=izz, + massSDF=massSDF, + posX=posX, + posY=posY, + posZ=posZ, + eulerX=eulerX, + eulerY=eulerY, + eulerZ=eulerZ, + friction=materialSolid.get(el.Label) or '', + ) ) - ) + except Exception as e: + print(200) return geometry diff --git a/insertion_vector_predicate/assembly b/insertion_vector_predicate/assembly new file mode 160000 index 0000000..395ee5b --- /dev/null +++ b/insertion_vector_predicate/assembly @@ -0,0 +1 @@ +Subproject commit 395ee5b638ccaa0cbe5b48101655560e49365195 diff --git a/insertion_vector_predicate/main.py b/insertion_vector_predicate/main.py new file mode 100644 index 0000000..ebdf9f7 --- /dev/null +++ b/insertion_vector_predicate/main.py @@ -0,0 +1,119 @@ +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/' + + +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): + return json.loads((open(path)).read()) + + def writeFile(data, filePath, fileName): + + file_to_open = filePath + fileName + + f = open(file_to_open, 'w', ) + + f.write(data) + + def readFile(path: str): + return open(path).read() + + def readFilesTypeFolder(pathFolder: str, fileType='.json'): + return os.listdir(pathFolder) + def readFolder(pathFolder: str): + return list( map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder) )) + + +def listGetFirstValue(iterable, default=False, pred=None): + return next(filter(pred, iterable), default) + + +def filterModels(filterModels, filterModelsDescription): + models = [] + for el in filterModelsDescription: + models.append(listGetFirstValue( + filterModels, None, lambda x: x.name == el)) + return models +def meshTransformation(): + 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): + args.print_helper() + if(aspDir[aspDir.__len__() - 1] != '/'): + aspDir += '/' + assemblys = FS.readFolder(aspDir + 'assembly') + assemblyDirNormalize = [] + for el in assemblys: + try: + + 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) + + + 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] + 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]]}) + planingObject = { + "time": t_plan, + "insertion_path": coords, + "status":status, + } + FS.writeFile(json.dumps(planingObject),el[0:el.__len__() - 8], 'insertion_path.json' ) + + try: + 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, + 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) + except Exception as e: + print(e) + + +meshTransformation() \ No newline at end of file diff --git a/insertion_vector_predicate/requirements.txt b/insertion_vector_predicate/requirements.txt new file mode 100644 index 0000000..4b67e47 --- /dev/null +++ b/insertion_vector_predicate/requirements.txt @@ -0,0 +1,2 @@ +spatialmath +scipy \ No newline at end of file diff --git a/sdf/requirements.txt b/sdf/requirements.txt deleted file mode 100644 index 1352d5e..0000000 --- a/sdf/requirements.txt +++ /dev/null @@ -1 +0,0 @@ -argparse