stability cad check

asp/.gitignore

@@ -0,0 +1 @@
+out

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

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(

asp/src/usecases/stability_check_usecase.py

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

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

cad_generation/.gitignore

@@ -1 +0,0 @@
-env.json

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

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

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

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

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
 
  

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

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

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

stability_check_predicate/main.py

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

stability_check_predicate/usecases/stability_check_usecase.py

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