Support "Assemble them all" trajectory generation from CAD

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