Add PDDL, 3D-assets & SDF-URDF generator from Blender Scene Config

insertion_vector_predicate/main.py

@@ -1,4 +1,14 @@
-# Алгоритм генерации графа с помощью вычисления векторов вставки при разборке изделия
+import os
+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/')
+
 from scipy.spatial.transform import Rotation
 import shutil
 from spatialmath import *
@@ -9,18 +19,32 @@ 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
+import trimesh
+
+import re
+def merge_meshes(meshes):
+    # Создание пустого меша
+    merged_mesh = trimesh.Trimesh()
+
+    # Объединение каждого меша в один
+    for mesh in meshes:
+        merged_mesh = trimesh.util.concatenate(
+            [merged_mesh, trimesh.load(mesh)])
+    i = True
+    while i:
+        if merged_mesh.fill_holes():
+            i = False
+ 
+      
+
+
+
+    return merged_mesh
+
+
 os.environ['OMP_NUM_THREADS'] = '1'
 
-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/')
-
+ 
 
 class FS:
     def readJSON(path: str):
@@ -43,6 +67,10 @@ class FS:
     def readFolder(pathFolder: str):
         return list(map(lambda el: pathFolder + '/' + el,  os.listdir(pathFolder)))
 
+    def createFolder(path: str):
+        if (not os.path.exists(path)):
+            return os.mkdir(path)
+
 
 def listGetFirstValue(iterable, default=False, pred=None):
     return next(filter(pred, iterable), default)
@@ -56,31 +84,44 @@ def filterModels(filterModels, filterModelsDescription):
     return models
 
 
+# mesh1 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube.obj')
+# mesh2 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube001.obj')
+
+
+# # Объединение мешей
+# merged_mesh = merge_meshes([mesh1, mesh2])
+
+# # Сохранение объединенного меша в файл
+# merged_mesh.export('merged.obj')
 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
+    # from argparse import ArgumentParser
+    # parser = ArgumentParser()
+    # parser.add_argument('--asp-path', type=str, required=True)
+    # args = parser.parse_args()
+    # aspDir = args.asp_dir
+
+    # # Коректировка пути до папки с генерацией ASP
+    # if (aspDir == None):
+    #     args.print_helper()
+    # if (aspDir[aspDir.__len__() - 1] != '/'):
+    #     aspDir += '/'
+    aspDir = '/home/idontsudo/framework/asp/out/'
+    sequences = FS.readJSON(aspDir + 'sequences.json').get('sequences')
 
-    # Коректировка пути до папки с генерацией ASP
-    if (aspDir == None):
-        args.print_helper()
-    if (aspDir[aspDir.__len__() - 1] != '/'):
-        aspDir += '/'
-    # Получение списка папок с .obj обьектами
-    assembles = FS.readFolder(aspDir + 'assembly')
     assemblyDirNormalize = []
-    for el in assembles:
-        try:
-            # Пост обработка .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 FS.readFolder(aspDir + 'assemblys'):
+       for e in FS.readFolder(el):
+                try:
+                    # Пост обработка .obj обьектов
+                    process_mesh(source_dir=e, target_dir=e +
+                                '/process/', subdivide=e, verbose=True)
+                    assemblyDirNormalize.append(e + '/process/')
+                except Exception as e:
+                    print('ERRROR:')
+                    print(e)
+ 
 
+    print(assemblyDirNormalize)
     for el in assemblyDirNormalize:
         asset_folder = os.path.join(project_base_dir, aspDir)
         assembly_dir = os.path.join(asset_folder, el)
@@ -113,13 +154,13 @@ def main():
         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,
+                planner_name='rrt',
+                step_size=None,
+                max_time=None,
+                seed=1,
                 return_path=True,
-                simplify=args.simplify,
-                render=args.render
+                simplify=False,
+                render=False
             )
 
             print(f'Status: {status}, planning time: {t_plan}')