Merge branch '6-model-config-xml' into 'master'

Gazebo Export Package restucturing Closes #5 and #6 See merge request robosphere/forks/ARBench!2
2022-02-07 20:54:17 +00:00 · 2022-02-07 20:54:17 +00:00 · 8241627436
commit 8241627436
parent 2111b33c75 e2da384085
5 changed files with 186 additions and 91 deletions
--- a/ARTools.py
+++ b/ARTools.py
@ -394,6 +394,16 @@ def exportPartInfoAndFeaturesDialogue():
                                 + str(unique_selected[0].Label)
                                 + " exported to " + str(ofile) + "\n")

+def exportGazeboModels():
+    import GazeboExport
+    doc = FreeCAD.activeDocument()
+    for obj in doc.Objects:
+        """Export solid shapes."""
+        if (isinstance(obj.Shape, Part.Solid) if hasattr(obj, 'Shape') else False):
+            GazeboExport.export_gazebo_model(obj, os.path.split(doc.FileName)[0], configs={})
+        elif isinstance(obj, Part.Feature):
+            FreeCAD.Console.PrintMessage('{0} part is not valid. It has a Compound type, but Solids there are hidden. Please convert it to single Solid'.format(obj.Label))
+

 ###################################################################
 # GUI Commands
@ -401,12 +411,19 @@ def exportPartInfoAndFeaturesDialogue():
 uidir = os.path.join(FreeCAD.getUserAppDataDir(),
                     "Mod", __workbenchname__, "UI")
 icondir = os.path.join(uidir, "icons")
+
 spawnClassCommand("ExportPartInfoAndFeaturesDialogueCommand",
                  exportPartInfoAndFeaturesDialogue,
                  {"Pixmap": str(os.path.join(icondir, "parttojson.svg")),
                   "MenuText": "Export info and featureframes",
                   "ToolTip": "Export part properties (placement, C.O.M) and feature frames"})

+spawnClassCommand("ExportGazeboModels",
+                  exportGazeboModels,
+                  {"Pixmap": str(os.path.join(icondir, "gazeboexport.svg")),
+                   "MenuText": "Export SDF-models to Gazebo",
+                   "ToolTip": "Export SDF-models for all solid parts"})
+

 ###################################################################
 # Information from primitive type
--- a/GazeboExport.py
+++ b/GazeboExport.py
@ -5,78 +5,75 @@ import collada
 from xml.etree import ElementTree as ET
 from xml.dom.minidom import parseString
 from math import radians as _radians
+import Part

-
-def export_gazebo_model(model_dir, configs={}):
-    doc = FreeCAD.activeDocument()
-    assembly_dir = os.path.split(doc.FileName)[0]
+def export_gazebo_model(obj, export_dir, configs={}):
+    name = obj.Label
+    
    scale = configs.get('scale', 0.001)
    scale_vec = FreeCAD.Vector([scale]*3)

-    for obj in doc.Objects:
-        if obj.isDerivedFrom("Part::Feature"):
-            name = obj.Label
-            density = configs.get('density', 1000)
+    density = configs.get('density', 1000)

-            bounding_box = obj.Shape.BoundBox
-            bounding_box.scale(*scale_vec)
+    bounding_box = obj.Shape.BoundBox
+    bounding_box.scale(*scale_vec)

-            global_pose_base = FreeCAD.Vector(bounding_box.XLength/2,
-                                         bounding_box.YLength/2,
-                                         bounding_box.ZLength/2)
-            global_pose_base -= bounding_box.Center
-            global_pose = FreeCAD.Placement()
-            global_pose.Base = global_pose_base
+    global_pose_base = FreeCAD.Vector(bounding_box.XLength/2,
+                                 bounding_box.YLength/2,
+                                 bounding_box.ZLength/2)
+    global_pose_base -= bounding_box.Center
+    global_pose = FreeCAD.Placement()
+    global_pose.Base = global_pose_base

-            model = Model(name=name, pose=global_pose)
-            model.self_collide = False
-            model.sdf_version = '1.5'
+    model = Model(name=name, pose=global_pose)
+    model.self_collide = False
+    model.sdf_version = '1.5'

-            shape = obj.Shape
-            mass = shape.Mass * scale**3 * density
-            com = shape.CenterOfMass * scale
-            inr = shape.MatrixOfInertia
-            inr.scale(*scale_vec*(scale**4) * density)
-            placement = shape.Placement
-            placement.Base.scale(*scale_vec)
+    shape = obj.Shape
+    mass = shape.Mass * scale**3 * density
+    com = shape.CenterOfMass * scale
+    inr = shape.MatrixOfInertia
+    inr.scale(*scale_vec*(scale**4) * density)
+    placement = shape.Placement
+    placement.Base.scale(*scale_vec)

-            mesh_file = os.path.join(model_dir, name, 'meshes')
-            mesh_file = os.path.splitext(mesh_file)[0] + name + '.dae'
-            mesh_dir = os.path.split(mesh_file)[0]
+    model_dir = os.path.join(export_dir, name)
+    mesh_dir = os.path.join(model_dir, 'meshes')
+    mesh_file = os.path.join(mesh_dir, name + '.dae')

-            os.makedirs(mesh_dir, exist_ok=True)
-            export_collada(doc, [obj], mesh_file, scale=scale, offset=com*-1)
+    os.makedirs(mesh_dir, exist_ok=True)
+    export_collada([obj], mesh_file, scale=scale, offset=com*-1)

-            pose = placement.copy()
-            pose.Base = com
+    pose = placement.copy()
+    pose.Base = com

-            pose_rpy = pose.copy()
-            pose_rpy.Base=(np.zeros(3))
+    pose_rpy = pose.copy()
+    pose_rpy.Base=(np.zeros(3))


-            inertia = Inertia(inertia=np.array(inr.A)[[0,1,2,5,6,10]])
-            inertial = Inertial(pose=pose_rpy,
-                                mass=mass,
-                                inertia=inertia)
+    inertia = Inertia(inertia=np.array(inr.A)[[0,1,2,5,6,10]])
+    inertial = Inertial(pose=pose_rpy,
+                        mass=mass,
+                        inertia=inertia)

-            package = configs.get('ros_package', name)
-            mesh_uri = os.path.join(package,
-                                    os.path.relpath(mesh_file, model_dir))
-            mesh_uri = os.path.normpath(mesh_uri)
+    mesh_uri = os.path.relpath(mesh_file, export_dir)
+    mesh_uri = os.path.normpath(mesh_uri)

-            visual = Visual(name=name+'_visual', mesh=mesh_uri)
-            collision = Collision(name=name+'_collision', mesh=mesh_uri)
+    visual = Visual(name=name+'_visual', mesh=mesh_uri)
+    collision = Collision(name=name+'_collision', mesh=mesh_uri)

-            link = Link(name=name,
-                        pose=pose,
-                        inertial=inertial,
-                        visual=visual,
-                        collision=collision)
-            model.links.append(link)
+    link = Link(name=name,
+                pose=pose,
+                inertial=inertial,
+                visual=visual,
+                collision=collision)
+    model.links.append(link)

-            with open(os.path.join(model_dir, name+'.sdf'), 'w') as sdf_file:
-                sdf_file.write(model.to_xml_string('sdf'))
+    with open(os.path.join(model_dir, 'model.sdf'), 'w') as sdf_file:
+        sdf_file.write(model.to_xml_string('sdf'))

+    with open(os.path.join(model_dir, 'model.config'), 'w') as config_file:
+        config_file.write(config(name, 'model.sdf', 'Author', 'Email', 'Comment', 'Version'))

 ###################################################################
 # Export helpers
@ -84,10 +81,9 @@ def export_gazebo_model(model_dir, configs={}):



-def export_collada(doc, exportList, filename, scale=1, quality=1, offset=np.zeros(3)):
+def export_collada(exportList, filename, scale=1, quality=1, offset=np.zeros(3)):
    '''FreeCAD collada exporter
-    doc - FreeCAD document
-    exportList - list of objects from doc
+    exportList - list of objects
    scale - scaling factor for the mesh
    quality - mesh tessellation quality
    offset - offset of the origin of the resulting mesh'''
@ -218,6 +214,28 @@ def pose_xyz(pose):
    xyz = pose.Base if hasattr(pose, 'Base') else pose
    return ' '.join([flt2str(i) for i in xyz])

+def config(model_name, sdf, author, email, desc, version):
+    top = ET.Element('model')
+    name = ET.SubElement(top, 'name')
+    name.text = model_name
+    ver = ET.SubElement(top, 'version')
+    ver.text = version
+    sdf_file = ET.SubElement(top, 'sdf')
+    sdf_file.text = sdf
+    sdf_file.set('version', '1.5')
+
+    author_tag = ET.SubElement(top, 'author')
+    author_name = ET.SubElement(author_tag, 'name')
+    author_name.text = author
+    email_address = ET.SubElement(author_tag, 'email')
+    email_address.text = email
+
+    description = ET.SubElement(top, 'description')
+    description.text = desc
+
+    dom = parseString(ET.tostring(top, encoding="unicode"))
+    return dom.toprettyxml(indent=' '*2)
+

 class SpatialEntity(object):
    '''A base class for sdf/urdf elements containing name, pose and urdf_pose'''
--- a/InitGui.py
+++ b/InitGui.py
@ -14,7 +14,8 @@ class ARBench(Workbench):
        self.framecommands = ["FrameCommand",
                              "AllPartFramesCommand",
                              "FeatureFrameCommand"]
-        self.toolcommands = ["ExportPartInfoAndFeaturesDialogueCommand"]
+        self.toolcommands = ["ExportPartInfoAndFeaturesDialogueCommand",
+                            "ExportGazeboModels"]
        self.appendToolbar("AR Frames", self.framecommands)
        self.appendToolbar("AR Tools", self.toolcommands)

--- a/README.md
+++ b/README.md
@ -1,5 +1,8 @@
 ![implementation preview](https://raw.githubusercontent.com/mahaarbo/ARBench/master/UI/icons/github_preview.png)
 # Arbench
+
+---___!!! USE WITH CAUTION! Plugin on heavy developement !!!___---
+
 Annotation for robotics bench. A FreeCAD workbench for annotating frames of interest, exporting these w.r.t. the part frame, and exporting part information.

 # Installation instructions
@ -19,6 +22,8 @@ This workbench supports versions of FreeCAD>0.16.

 # Usage

+## Export meta-data for part's feature frames
+
 1. Click a small feature e.g. a circle
 2. Press the feature frame creator (cone with a magnifying glass on it icon)
 3. Chose type of feature to create
@ -29,41 +34,15 @@ This workbench supports versions of FreeCAD>0.16.
 8. Use the json with whatever you want. E.g. [`arbench_part_publisher`](https://github.com/mahaarbo/arbench_part_publisher)


-# Freecad to Gazebo exporter
+## Generate part's model packages for Gazebo simulator

-To generate SDF and URDF model from freecad assembly use python call:
-
-```python
-freecad_exporter.export_gazebo_model(freecad_assembly_file, model_destination_folder, config)
+To generate SDF model packages from FreeCAD Document just press "Gazebo Export" button in ARBench UI. It will create folder for every `Solid` part in Document (`Compound` parts currently doesn't supported) with such structure
 ```
-Note: Only links and joints are generated in the SDF model. To use the model with ros, use the URDF model.
+name_of_part
+├── model.sdf
+├── meshes
+│   └── part.dae
+└── model.config

-## Config specification
-```json
-{
-    "name": "robot_name",
-    "joints_limits": {"upper": 90, "lower": -90, "effort": 10, "velocity": 5},
-    "transmission": {
-        "type": "transmission_interface/SimpleTransmission",
-        "hardware_interface": "hardware_interface/PositionJointInterface"
-    },
-    "joints_config": {
-        "type": "position_controllers/JointGroupPositionController",
-        "grouped": true
-    },
-    "joints_pid": {"p": 20.0, "i": 10.0, "d": 0.0, "i_clamp": 0.0},
-    "root_link": "base_link",
-    "ros_package": "humanoid_17dof_description",
-    "sdf_only": false,
-    "export": true
-}
 ```
-
-**sdf_only**: Export only SDF.
-
-**export**: Export mesh files.
-
-## Future plans
-* Extend collada exporter to export materials from assemblies.
-* Create a FreeCAD workbench to interactively assign joints and export to gazebo.
-* Support any valid structures of assemblies.
+This packages will placed by default in your FreeCAD Document's folder and could be moved to gazebo model's folder for using them in sumulator.
--- a/UI/icons/gazeboexport.svg
+++ b/UI/icons/gazeboexport.svg
@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="300"
+   height="300"
+   id="svg6408"
+   version="1.1"
+   inkscape:version="0.48.4 r9939"
+   sodipodi:docname="New document 18">
+  <defs
+     id="defs6410" />
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="1.979899"
+     inkscape:cx="170.62581"
+     inkscape:cy="178.84029"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer1"
+     showgrid="false"
+     inkscape:window-width="1749"
+     inkscape:window-height="1203"
+     inkscape:window-x="0"
+     inkscape:window-y="0"
+     inkscape:window-maximized="0" />
+  <metadata
+     id="metadata6413">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+        <dc:title></dc:title>
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="Layer 1"
+     inkscape:groupmode="layer"
+     id="layer1"
+     transform="translate(0,-752.36218)">
+    <g
+       transform="matrix(1.7219458,0,0,1.7219458,-240.52546,490.39612)"
+       id="g6387">
+      <path
+         inkscape:connector-curvature="0"
+         id="path4004"
+         d="m 164.74024,205.99021 1.89902,67.41531 59.81923,38.92997 0.94951,-34.65717 35.13192,-19.46498 z"
+         style="fill:#ffffff;fill-opacity:1;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+      <path
+         d="m 222.38424,244.55563 2.02975,1.25975 c 0.72813,0.45312 1.55275,0.67775 2.38038,0.67775 0.82574,0 1.65337,-0.22463 2.38337,-0.68113 l 2.03225,-1.26463 20.60538,12.75975 -27.51213,17.13288 c -1.32725,0.82362 -2.13288,2.27775 -2.1265,3.84512 l 0.0805,28.07713 -49.7045,-30.86425 49.8315,-30.94237 z m -54.03223,-33.54926 45.47998,28.23975 -45.47998,28.23825 0,-56.478 z m 125.63548,28.14788 c -0.004,-1.63325 -0.8925,-3.13962 -2.32625,-3.933 -1.43,-0.792 -3.17875,-0.74712 -4.5675,0.11712 l -26.73338,16.648 -20.60312,-12.75887 52.209,-32.51463 c 1.325,-0.82512 2.13125,-2.27537 2.13125,-3.83637 -0.004,-1.55763 -0.8125,-3.00838 -2.1375,-3.82963 l -62.79113,-38.88062 c -1.4575,-0.90375 -3.29824,-0.90125 -4.75487,0.002 l -62.79292,38.99137 c -1.3252,0.82175 -2.13233,2.272 -2.13233,3.83301 0,0.40574 0,72.09862 0,72.50487 0,1.56062 0.80713,3.01025 2.13233,3.8345 l 62.79292,38.98977 c 0.0307,0.019 0.0625,0.0303 0.0929,0.0478 0.0346,0.0186 0.0634,0.0449 0.0991,0.0659 0.0708,0.0376 0.1455,0.0664 0.21963,0.10254 0.0732,0.0376 0.14499,0.0737 0.22224,0.10645 0.11713,0.0474 0.23676,0.0889 0.35638,0.125 0.0709,0.0249 0.13925,0.0513 0.2115,0.0688 0.13425,0.041 0.27187,0.0625 0.40863,0.0884 0.0571,0.0117 0.11724,0.0254 0.17574,0.0288 0.19688,0.0303 0.3955,0.0439 0.59238,0.0439 l 0,0 c 0,0 0.001,0 0.002,0 0.40926,0 0.82188,-0.0576 1.21825,-0.16748 0.004,-0.004 0.006,-0.004 0.006,-0.004 0.15387,-0.041 0.30525,-0.10254 0.45412,-0.16113 0.0449,-0.0186 0.0927,-0.0298 0.13676,-0.0474 0.126,-0.0552 0.24262,-0.12891 0.36225,-0.19532 0.066,-0.0361 0.13574,-0.0649 0.20074,-0.10253 l 0.002,-0.003 c 0.003,0 0.003,0 0.003,0 l 0.12062,-0.0762 62.66538,-38.91018 c 1.33125,-0.82762 2.13625,-2.28175 2.13375,-3.84912 l -0.11,-36.32913"
+         id="path3115"
+         style="fill:#231f20;fill-opacity:1;fill-rule:nonzero;stroke:none"
+         inkscape:connector-curvature="0" />
+      <path
+         d="m 231.20499,280.76812 0.0772,25.623 53.784,-33.398 -0.075,-25.71625 -53.78625,33.49125"
+         id="path3117"
+         style="fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
+         inkscape:connector-curvature="0" />
+      <path
+         d="m 172.55274,202.99313 54.23975,33.67824 54.23925,-33.77687 -54.23437,-33.58575 -54.24463,33.68438"
+         id="path3119"
+         style="fill:#f58113;fill-opacity:1;fill-rule:nonzero;stroke:none"
+         inkscape:connector-curvature="0" />
+    </g>
+  </g>
+</svg>