Support "Assemble them all" trajectory generation from CAD

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "insertion_vector_predicate/assembly"]
+	path = insertion_vector_predicate/assembly
+	url = https://github.com/yunshengtian/Assemble-Them-All

asp/requirements.txt

@@ -0,0 +1,5 @@
+argparse
+matplotlib
+pybullet
+argparse
+xmlformatter

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")),

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)

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

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)
+
+
+ 

cg/freecad/Frames/model/files_generator.py

@@ -10,3 +10,4 @@ class FolderGenerator(Enum):
     MESHES = 'meshes'
     ASSETS = 'assets'
     SDF = 'sdf'
+    ASSEMBlY = 'assembly'

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)

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)

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)

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',)
              

cg/freecad/Frames/usecases/assembly_parse_usecase.py

@@ -1,17 +1,37 @@
 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:
+        if (self._asm.__len__() == 0):
             self.initParse()
         pass
 
-    
-
     def initParse(self):
         for el in App.ActiveDocument.Objects:
             if (is_object_solid(el)):
@@ -28,7 +48,6 @@ 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:
@@ -37,6 +56,3 @@ class AssemblyParseUseCase:
 
     def getLinkedProperty(self):
         return self._asm
-
-  
-

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
+            
+ 

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)):
+                match type.value:
+                    case EXPORT_TYPES.STL.value:
                         Mesh.export([el], path + '/' + FolderGenerator.SDF.value +
-                            '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae')
+                                    '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.stl')
                         meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
-                    '/' + el.Label + '.dae'
+                            '/' + 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

cg/freecad/Frames/usecases/geometry_usecase.py

@@ -6,7 +6,9 @@ from helper.is_solid import is_object_solid
 class GeometryUseCase:
     def call() -> dict:
         labels = []
+        Error = False
         for el in App.ActiveDocument.Objects:
+            try:
                     
                 if is_object_solid(el):
                     labels.append(el.Label)
@@ -50,5 +52,7 @@ class GeometryUseCase:
                     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}

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) == '<App::MaterialObjectPython object>':   
@@ -13,9 +15,23 @@ class SdfGeometryUseCase:
                 for i in el.References:
                     materialSolid[i[0].Label] = friction
         geometry = []
+        try:
             for el in App.ActiveDocument.Objects:
                 if is_object_solid(el):
-                com = el.Shape.CenterOfMass
+                    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
@@ -55,6 +71,8 @@ class SdfGeometryUseCase:
                             friction=materialSolid.get(el.Label) or '',
                         )
                     )
+        except Exception as e:
+            print(200)
         return geometry
 
  

insertion_vector_predicate/assembly

@@ -0,0 +1 @@
+Subproject commit 395ee5b638ccaa0cbe5b48101655560e49365195

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()

insertion_vector_predicate/requirements.txt

@@ -0,0 +1,2 @@
+spatialmath
+scipy

sdf/requirements.txt

@@ -1 +0,0 @@
-argparse