diff --git a/ARTools.py b/ARTools.py index 42e7daf..8dcbbb9 100644 --- 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 diff --git a/GazeboExport.py b/GazeboExport.py index a1f16f7..e33680e 100644 --- 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''' diff --git a/InitGui.py b/InitGui.py index a8cf245..e1d664e 100644 --- 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) diff --git a/README.md b/README.md index fcc89f1..3b23900 100644 --- 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. \ No newline at end of file diff --git a/UI/icons/gazeboexport.svg b/UI/icons/gazeboexport.svg new file mode 100644 index 0000000..944756d --- /dev/null +++ b/UI/icons/gazeboexport.svg @@ -0,0 +1,80 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + +