Merge branch 'freecad0.19' into 'master'

Merge freecad_to_gazebo to ARBench & FreeCAD 0.19 support

See merge request robosphere/forks/ARBench!1

ARTools.py

@@ -49,7 +49,7 @@ def boundingBox2list(bb, scale=1e-3):
 
 def principalProperties2dict(pp, scale=1e-3):
     npp = {}
-    for key, value in pp.iteritems():
+    for key, value in pp.items():
         if type(value) is FreeCAD.Vector:
             npp[key.lower()] = vector2list(value, scale=1e-3)
         else:
@@ -179,7 +179,7 @@ def exportPartInfo(obj, ofile):
         ofile = ofile + ".json"
 
     partprops = getLocalPartProps(obj)
-    with open(ofile, "wb") as propfile:
+    with open(ofile, "w", encoding="utf8") as propfile:
         json.dump(partprops, propfile, indent=1, separators=(',', ': '))
     return True
 
@@ -192,11 +192,11 @@ def appendPartInfo(obj, ofile):
     For more information on principal properties, see TopoShape in OCCT
     documentation.
     """
-    with open(ofile, "rb") as propfile:
+    with open(ofile, "r", encoding="utf8") as propfile:
         partprops = json.load(propfile)
     new_props = getLocalPartProps(obj)
     partprops.update(new_props)
-    with open(ofile, "wb") as propfile:
+    with open(ofile, "w", encoding="utf8") as propfile:
         json.dump(partprops, propfile, indent=1, separators=(',', ': '))
     return True
 
@@ -205,7 +205,7 @@ def exportFeatureFrames(obj, ofile):
     """Exports feature frames attached to a part."""
     # Get the feature frames
     import ARFrames
-    ff_check = lambda x: isinstance(x.Proxy, ARFrames.FeatureFrame)
+    ff_check = lambda x: isinstance(x.Proxy, ARFrames.FeatureFrame) if hasattr(x, 'Proxy') else False
     ff_list = filter(ff_check, obj.InList)
     ff_named = {ff.Label: ff.Proxy.getDict() for ff in ff_list}
     feature_dict = {"features": ff_named}
@@ -216,7 +216,7 @@ def exportFeatureFrames(obj, ofile):
         os.makedirs(odir)
     if not of.lower().endswith(".json"):
         ofile = ofile + ".json"
-    with open(ofile, "wb") as propfile:
+    with open(ofile, "w", encoding="utf8") as propfile:
         json.dump(feature_dict, propfile, indent=1, separators=(',', ': '))
     return True
 
@@ -226,9 +226,9 @@ def appendFeatureFrames(obj, ofile):
     file."""
     # Get the feature frames
     import ARFrames
-    with open(ofile, "rb") as propfile:
+    with open(ofile, "r", encoding="utf8") as propfile:
         partprops = json.load(propfile)
-    ff_check = lambda x: isinstance(x.Proxy, ARFrames.FeatureFrame)
+    ff_check = lambda x: isinstance(x.Proxy, ARFrames.FeatureFrame) if hasattr(x, 'Proxy') else False 
     ff_list = filter(ff_check, obj.InList)
     ff_named = {ff.Label: ff.Proxy.getDict() for ff in ff_list}
     feature_dict = {"features": ff_named}
@@ -236,7 +236,7 @@ def appendFeatureFrames(obj, ofile):
         partprops.update(feature_dict)
     else:
         partprops["features"].update(feature_dict["features"])
-    with open(ofile, "wb") as propfile:
+    with open(ofile, "w", encoding="utf8") as propfile:
         json.dump(partprops, propfile, indent=1, separators=(',', ': '))
     return True
 
@@ -270,10 +270,8 @@ def exportPartInfoDialogue():
     if os.path.exists(ofile):
         msgbox = QtGui.QMessageBox()
         msgbox.setText("File already exists. We can overwrite the file, or add the information/rewrite only relevant sections.")
-        append_button = msgbox.addButton(unicode("Append"),
-                                         QtGui.QMessageBox.YesRole)
-        overwrite_button = msgbox.addButton(unicode("Overwrite"),
-                                            QtGui.QMessageBox.NoRole)
+        append_button = msgbox.addButton("Append", QtGui.QMessageBox.YesRole)
+        overwrite_button = msgbox.addButton("Overwrite", QtGui.QMessageBox.NoRole)
         msgbox.exec_()
         if msgbox.clickedButton() == append_button:
             NEWFILE = False
@@ -296,6 +294,7 @@ def exportPartInfoDialogue():
 def exportFeatureFramesDialogue():
     """Spawns a dialogue window for a part's feature frames to be exported."""
     # Select only true parts
+    import ARFrames
     s = FreeCADGui.Selection.getSelection()
     FreeCADGui.Selection.clearSelection()
     if len(s) == 0:
@@ -303,7 +302,7 @@ def exportFeatureFramesDialogue():
         return False
     unique_selected = []
     for item in s:
-        if item not in unique_selected and isinstance(item, Part.Feature):
+        if item not in unique_selected and isinstance(item.Proxy, ARFrames.FeatureFrame):
             # Ensuring that we are parts
             unique_selected.append(item)
             FreeCADGui.Selection.addSelection(item)
@@ -322,10 +321,8 @@ def exportFeatureFramesDialogue():
     if os.path.exists(ofile):
         msgbox = QtGui.QMessageBox()
         msgbox.setText("File already exists. We can overwrite the file, or add the information/rewrite only relevant sections.")
-        append_button = msgbox.addButton(unicode("Append"),
-                                         QtGui.QMessageBox.YesRole)
-        overwrite_button = msgbox.addButton(unicode("Overwrite"),
-                                            QtGui.QMessageBox.NoRole)
+        append_button = msgbox.addButton("Append", QtGui.QMessageBox.YesRole)
+        overwrite_button = msgbox.addButton("Overwrite", QtGui.QMessageBox.NoRole)
         msgbox.exec_()
         if msgbox.clickedButton() == append_button:
             NEWFILE = False
@@ -346,17 +343,19 @@ def exportFeatureFramesDialogue():
 
 def exportPartInfoAndFeaturesDialogue():
     """Spawns a dialogue window for exporting both."""
+    import ARFrames
     s = FreeCADGui.Selection.getSelection()
     FreeCADGui.Selection.clearSelection()
     if len(s) == 0:
-        FreeCAD.Console.PrintError("No part selected.")
+        FreeCAD.Console.PrintError("No part selected")
         return False
     unique_selected = []
     for item in s:
-        if item not in unique_selected and isinstance(item, Part.Feature):
+        if item not in unique_selected:
             # Ensuring that we are parts
             unique_selected.append(item)
             FreeCADGui.Selection.addSelection(item)
+            FreeCAD.Console.PrintMessage("Added for export "+str(item.FullName)+"\n")
     # Fix wording
     textprompt = "Save the part info and feature frames attached to the part"
     if len(unique_selected) > 1:
@@ -372,10 +371,8 @@ def exportPartInfoAndFeaturesDialogue():
     if os.path.exists(ofile):
         msgbox = QtGui.QMessageBox()
         msgbox.setText("File already exists. We can overwrite the file, or add the information/rewrite only relevant sections.")
-        append_button = msgbox.addButton(unicode("Append"),
-                                         QtGui.QMessageBox.YesRole)
-        overwrite_button = msgbox.addButton(unicode("Overwrite"),
-                                            QtGui.QMessageBox.NoRole)
+        append_button = msgbox.addButton("Append", QtGui.QMessageBox.YesRole)
+        overwrite_button = msgbox.addButton("Overwrite", QtGui.QMessageBox.NoRole)
         msgbox.exec_()
         if msgbox.clickedButton() == append_button:
             NEWFILE = False

GazeboExport.py

@@ -0,0 +1,563 @@
+import FreeCAD, Mesh, os, numpy as np
+import yaml
+import argparse
+import collada
+from xml.etree import ElementTree as ET
+from xml.dom.minidom import parseString
+from math import radians as _radians
+
+
+def export_gazebo_model(model_dir, configs={}):
+    doc = FreeCAD.activeDocument()
+    assembly_dir = os.path.split(doc.FileName)[0]
+    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)
+
+            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
+
+            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)
+
+            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]
+
+            os.makedirs(mesh_dir, exist_ok=True)
+            export_collada(doc, [obj], mesh_file, scale=scale, offset=com*-1)
+
+            pose = placement.copy()
+            pose.Base = com
+
+            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)
+
+            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)
+
+            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)
+
+            with open(os.path.join(model_dir, name+'.sdf'), 'w') as sdf_file:
+                sdf_file.write(model.to_xml_string('sdf'))
+
+
+###################################################################
+# Export helpers
+###################################################################
+
+
+
+def export_collada(doc, exportList, filename, scale=1, quality=1, offset=np.zeros(3)):
+    '''FreeCAD collada exporter
+    doc - FreeCAD document
+    exportList - list of objects from doc
+    scale - scaling factor for the mesh
+    quality - mesh tessellation quality
+    offset - offset of the origin of the resulting mesh'''
+
+    colmesh = collada.Collada()
+    colmesh.assetInfo.upaxis = collada.asset.UP_AXIS.Z_UP
+    objind = 0
+    scenenodes = []
+
+    for obj in exportList:
+        bHandled = False
+        if obj.isDerivedFrom("Part::Feature"):
+            bHandled = True
+            m = obj.Shape.tessellate(quality)
+            vindex = []
+            nindex = []
+            findex = []
+            # vertex indices
+            for v in m[0]:
+                vindex.extend([a*scale+b for a, b in zip(v, offset)])
+            # normals
+            for f in obj.Shape.Faces:
+                n = f.normalAt(0,0)
+                for i in range(len(f.tessellate(quality)[1])):
+                    nindex.extend([n.x,n.y,n.z])
+            # face indices
+            for i in range(len(m[1])):
+                f = m[1][i]
+                findex.extend([f[0],i,f[1],i,f[2],i])
+        elif obj.isDerivedFrom("Mesh::Feature"):
+            bHandled = True
+            print("exporting mesh ",obj.Name, obj.Mesh)
+            m = obj.Mesh
+            vindex = []
+            nindex = []
+            findex = []
+            # vertex indices
+            for v in m.Topology[0]:
+                vindex.extend([a*scale+b for a, b in zip(v, offset)])
+            # normals
+            for f in m.Facets:
+                n = f.Normal
+                nindex.extend([n.x,n.y,n.z])
+            # face indices
+            for i in range(len(m.Topology[1])):
+                f = m.Topology[1][i]
+                findex.extend([f[0],i,f[1],i,f[2],i])
+
+        if bHandled:
+            vert_src = collada.source.FloatSource("cubeverts-array"+str(objind),
+                                                  np.array(vindex),
+                                                  ('X', 'Y', 'Z'))
+            normal_src = collada.source.FloatSource("cubenormals-array"+str(objind),
+                                                    np.array(nindex),
+                                                    ('X', 'Y', 'Z'))
+            geom = collada.geometry.Geometry(colmesh,
+                                             "geometry"+str(objind),
+                                             obj.Label,
+                                             [vert_src, normal_src])
+
+            input_list = collada.source.InputList()
+            input_list.addInput(0, 'VERTEX', "#cubeverts-array"+str(objind))
+            input_list.addInput(1, 'NORMAL', "#cubenormals-array"+str(objind))
+            triset = geom.createTriangleSet(np.array(findex),
+                                            input_list,
+                                            "materialref")
+            geom.primitives.append(triset)
+            colmesh.geometries.append(geom)
+
+            geomnode = collada.scene.GeometryNode(geom)
+            node = collada.scene.Node("node"+str(objind), children=[geomnode])
+
+            #TODO: Add materials handling
+            scenenodes.append(node)
+
+        objind += 1
+
+    scene = collada.scene.Scene("scene", scenenodes)
+    colmesh.scenes.append(scene)
+    colmesh.scene = scene
+
+    colmesh.write(filename)
+    print("file %s successfully created\n" % filename)
+
+
+###################################################################
+# Conversion Helpers
+###################################################################
+
+def deg2rad(d):
+    '''Converts degrees to radians'''
+    return _radians(d)
+
+def flt2str(f):
+    '''Converts floats to formatted string'''
+    return '{:.6f}'.format(f)
+
+
+###################################################################
+# Model Helpers
+###################################################################
+
+def add_poses(p1, p2):
+    return FreeCAD.Placement(p1.toMatrix() + p2.toMatrix())
+
+def subtract_poses(p1, p2):
+    return FreeCAD.Placement(p1.toMatrix() + p2.toMatrix().inverse())
+
+def pose_to_xml(pose, fmt='sdf'):
+    '''Converts a pose/freecad placement/ to xml element
+    with tag "pose" for sdf and "origin" for urdf'''
+    xyz = pose.Base
+    rpy = pose.Rotation.toEuler()
+
+    if fmt == 'urdf':
+        args = {'xyz': ' '.join([flt2str(i) for i in xyz]),
+                'rpy': ' '.join([flt2str(deg2rad(j)) for j in rpy])}
+        return ET.Element('origin', args)
+
+    pose_elem = ET.Element('pose')
+    pose_elem.text = ' '.join([flt2str(i) for i in xyz]
+                              + [flt2str(deg2rad(j)) for j in rpy])
+
+    return pose_elem
+
+def pose_xyz(pose):
+    '''Returns the xyz/Base portion of a pose as string'''
+    xyz = pose.Base if hasattr(pose, 'Base') else pose
+    return ' '.join([flt2str(i) for i in xyz])
+
+
+class SpatialEntity(object):
+    '''A base class for sdf/urdf elements containing name, pose and urdf_pose'''
+    def __init__(self, **kwargs):
+        self.name = kwargs.get('name', '')
+        self.pose = kwargs.get('pose', FreeCAD.Placement())
+        self.global_pose = kwargs.get('global_pose', FreeCAD.Placement())
+        self.urdf_pose = kwargs.get('urdf_pose', FreeCAD.Placement())
+
+        self.formats = ['sdf', 'urdf']
+
+    def to_xml(self, fmt='sdf'):
+        '''Call this to check if a format is supported or not'''
+        if not fmt in self.formats:
+            raise Exception('Invalid export format')
+
+
+class Model(SpatialEntity):
+    '''A class representing a model/robot'''
+    def __init__(self, **kwargs):
+        super(Model, self).__init__(**kwargs)
+        self.static = kwargs.get('static', False)
+        self.self_collide = kwargs.get('self_collide', False)
+        self.sdf_version = kwargs.get('sdf_version', 1.5)
+
+        self.links = []
+        self.joints = []
+
+        if 'link' in kwargs:
+            self.links.append(kwargs.get('link', Link()))
+        self.links.extend(kwargs.get('links', []))
+
+        if 'joint' in kwargs:
+            self.joints.append(kwargs.get('joint', Joint()))
+        self.joints.extend(kwargs.get('joints', []))
+
+    def get_link(self, link_name):
+        for link in self.links:
+            if link_name == link.name:
+                return link
+
+    def get_joint(self, joint_name):
+        pass
+
+    def get_root_link(self):
+        root_link = None
+        for link in self.links:
+            if not link.parent_joint:
+                root_link = link
+        return root_link
+
+    def build_tree(self):
+        for joint in self.joints:
+            joint.parent_link = self.get_link(joint.parent)
+            if not joint.parent_link:
+                raise Exception('Parent not found for joint %s' % joint.name)
+            joint.parent_link.child_joints.append(joint)
+
+            joint.child_link = self.get_link(joint.child)
+            if not joint.child_link:
+                raise Exception('Child not found for joint %s' % joint.name)
+            joint.child_link.parent_joint = joint
+
+    def calculate_global_poses(self):
+        for link in self.links:
+            link.global_pose = add_poses(self.pose, link.pose)
+        for joint in self.joints:
+            joint.global_pose = add_poses(joint.child_link.global_pose, joint.pose)
+
+    def to_xml(self, fmt='sdf'):
+        '''returns xml element of a model/robot'''
+        super(Model, self).to_xml(fmt)
+
+        self.build_tree()
+        self.calculate_global_poses()
+
+        tag = 'robot' if fmt=='urdf' else 'model'
+        model = ET.Element(tag, name=self.name)
+
+        if fmt == 'sdf':
+            model.append(pose_to_xml(self.pose))
+            static = ET.SubElement(model, 'static')
+            static.text = str(self.static).lower()
+            self_collide = ET.SubElement(model, 'self_collide')
+            self_collide.text = str(self.self_collide).lower()
+        else:
+            model.set('static', str(self.static).lower())
+
+            root_link = self.get_root_link()
+            if not root_link:
+                raise Exception("Couldn't find root link")
+            model.append(ET.Element('link', name=root_link.name+'_root'))
+        for link in self.links:
+            model.append(link.to_xml(fmt))
+
+        if fmt=='urdf':
+            root_joint = ET.Element('joint',
+                                    {"name": root_link.name+'_root',
+                                     "type": "fixed"})
+            root_joint.append(pose_to_xml(root_link.global_pose, fmt))
+            ET.SubElement(root_joint, 'parent', link= root_link.name+'_root')
+            ET.SubElement(root_joint, 'child', link= root_link.name)
+            model.append(root_joint)
+
+        for joint in self.joints:
+            model.append(joint.to_xml(fmt))
+
+        if fmt == 'sdf':
+            sdf = ET.Element('sdf', version=str(self.sdf_version))
+            sdf.append(model)
+            return sdf
+
+        return model
+
+    def to_xml_string(self, fmt='sdf', header=True):
+        dom = parseString(ET.tostring(self.to_xml(fmt)))
+        return dom.toprettyxml(indent=' '*2)
+
+
+class Inertia(object):
+    '''A clss representing an inertia element'''
+    def __init__(self, **kwargs):
+        self.ixx = kwargs.get('ixx', 0)
+        self.ixy = kwargs.get('ixy', 0)
+        self.ixz = kwargs.get('ixz', 0)
+        self.iyy = kwargs.get('iyy', 0)
+        self.iyz = kwargs.get('iyz', 0)
+        self.izz = kwargs.get('izz', 0)
+        if 'inertia' in kwargs:
+            self.ixx, self.ixy, self.ixz = kwargs.get('inertia', [0]*6)[:3]
+            self.iyy, self.iyz, self.izz = kwargs.get('inertia', [0]*6)[3:]
+        self.coords = 'ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'
+
+    def to_xml(self, fmt='sdf'):
+        '''returns inetria xml element'''
+        inertia = ET.Element('inertia')
+        for coord in self.coords:
+            if fmt == 'sdf':
+                elem = ET.SubElement(inertia, coord)
+                elem.text = flt2str(getattr(self, coord, 0))
+            else:
+                inertia.set(coord, flt2str(getattr(self, coord, 0)))
+        return inertia
+
+
+class Inertial(SpatialEntity):
+    '''A class representing an inertial element'''
+    def __init__(self, **kwargs):
+        super(Inertial, self).__init__(**kwargs)
+        self.mass = kwargs.get('mass', 0)
+        self.inertia = kwargs.get('inertia', Inertia())
+
+    def to_xml(self, fmt='sdf'):
+        '''returns inertial xml element'''
+        super(Inertial, self).to_xml(fmt)
+        inertial = ET.Element('inertial')
+        pose = self.pose if fmt=='sdf' else self.urdf_pose
+        inertial.append(pose_to_xml(pose, fmt=fmt))
+        mass = ET.SubElement(inertial, 'mass')
+        if fmt == 'sdf':
+            mass.text = flt2str(self.mass)
+        else:
+            mass.set('value', flt2str(self.mass))
+        inertial.append(self.inertia.to_xml(fmt=fmt))
+
+        return inertial
+
+
+class Geom(SpatialEntity):
+    '''A base class for collision and visual classes'''
+    def __init__(self, **kwargs):
+        super(Geom, self).__init__(**kwargs)
+        self.mesh = kwargs.get('mesh', '')
+        self.type = kwargs.get('type', 'visual')
+
+    def to_xml(self, fmt='sdf'):
+        '''returns visual or collision xml element'''
+        super(Geom, self).to_xml(fmt)
+        elem = ET.Element(self.type, name=self.name)
+        pose = self.pose if fmt=='sdf' else self.urdf_pose
+        elem.append(pose_to_xml(pose, fmt=fmt))
+        geom = ET.SubElement(elem, 'geometry')
+        mesh = ET.SubElement(geom, 'mesh')
+        if fmt=='urdf':
+            mesh.set('filename', 'package://' + self.mesh)
+        else:
+            uri = ET.SubElement(mesh, 'uri')
+            uri.text = 'model://' + self.mesh
+
+        return elem
+
+
+class Visual(Geom):
+    '''A class representing a visual element'''
+    def __init__(self, **kwargs):
+        super(Visual, self).__init__(type='visual', **kwargs)
+
+
+class Collision(Geom):
+    '''A class representing a collision element'''
+    def __init__(self, **kwargs):
+        super(Collision, self).__init__(type='collision', **kwargs)
+
+
+class Link(SpatialEntity):
+    '''A class representing a link element'''
+    def __init__(self, **kwargs):
+        super(Link, self).__init__(**kwargs)
+        self.inertial = kwargs.get('inertial', Inertial())
+        self.visuals = []
+        self.collisions = []
+
+        self.parent_joint = None
+        self.child_joints = []
+
+        if 'visual' in kwargs:
+            self.visuals.append(kwargs.get('visual', Visual()))
+        self.visuals.extend(kwargs.get('visuals', []))
+
+        if 'collision' in kwargs:
+            self.visuals.append(kwargs.get('collision', Collision()))
+        self.collisions.extend(kwargs.get('collisions', []))
+
+    def to_xml(self, fmt='sdf'):
+        '''returns link xml element'''
+        super(Link, self).to_xml(fmt)
+        link = ET.Element('link', name=self.name)
+
+        if self.parent_joint:
+            self.urdf_pose = subtract_poses(self.global_pose,
+                                           self.parent_joint.global_pose)
+        else:
+            self.urdf_pose = FreeCAD.Placement()
+
+        if fmt == 'sdf':
+            link.append(pose_to_xml(self.pose, fmt=fmt))
+
+        self.inertial.urdf_pose = add_poses(self.inertial.pose, self.urdf_pose)
+        link.append(self.inertial.to_xml(fmt=fmt))
+
+        for visual in self.visuals:
+            visual.urdf_pose = self.urdf_pose.copy()
+            link.append(visual.to_xml(fmt=fmt))
+        for collision in self.collisions:
+            collision.urdf_pose = self.urdf_pose.copy()
+            link.append(collision.to_xml(fmt=fmt))
+
+        return link
+
+
+class Axis(SpatialEntity):
+    '''A class representing an axis element'''
+    def __init__(self, **kwargs):
+        super(Axis, self).__init__(**kwargs)
+        self.lower_limit = kwargs.get('lower_limit', 0)
+        self.upper_limit = kwargs.get('upper_limit', 0)
+        self.effort_limit = kwargs.get('effort_limit', 0)
+        self.velocity_limit = kwargs.get('velocity_limit', 0)
+        self.friction = kwargs.get('friction', 0)
+        self.damping = kwargs.get('damping', 0)
+        self.use_parent_frame = kwargs.get('use_parent_frame', False)
+
+    def to_xml(self, fmt='sdf'):
+        '''returns an axis xml element for sdf
+        or an array of axis and limit xml elements for urdf'''
+        super(Axis, self).to_xml(fmt)
+
+        axis = ET.Element('axis')
+        if fmt=='sdf':
+            xyz = ET.SubElement(axis, 'xyz')
+            xyz.text = pose_xyz(self.pose)
+            limit = ET.SubElement(axis, 'limit')
+            lower = ET.SubElement(limit, 'lower')
+            lower.text = flt2str(deg2rad(self.lower_limit))
+            upper = ET.SubElement(limit, 'upper')
+            upper.text = flt2str(deg2rad(self.upper_limit))
+            effort = ET.SubElement(limit, 'effort')
+            effort.text = flt2str(self.effort_limit)
+            velocity = ET.SubElement(limit, 'velocity')
+            velocity.text = flt2str(self.velocity_limit)
+            dynamics = ET.SubElement(axis, 'dynamics')
+            friction = ET.SubElement(dynamics, 'friction')
+            friction.text = flt2str(self.friction)
+            damping = ET.SubElement(dynamics, 'damping')
+            damping.text = flt2str(self.damping)
+            use_parent_frame = ET.SubElement(axis, 'use_parent_model_frame')
+            use_parent_frame.text = str(self.use_parent_frame).lower()
+        else:
+            axis.set('xyz', pose_xyz(self.pose))
+            axis.set('use_parent_model_frame', str(self.use_parent_frame).lower())
+            limit = ET.Element('limit')
+            limit.set('lower', flt2str(deg2rad(self.lower_limit)))
+            limit.set('upper', flt2str(deg2rad(self.upper_limit)))
+            limit.set('effort', flt2str(self.effort_limit))
+            limit.set('velocity', flt2str(self.velocity_limit))
+
+            dynamics = ET.Element('dynamics')
+            dynamics.set('friction', flt2str(self.friction))
+            dynamics.set('damping', flt2str(self.damping))
+
+            return [axis, limit, dynamics]
+        return axis
+
+
+class Joint(SpatialEntity):
+    '''A class representing a joint element'''
+    def __init__(self, **kwargs):
+        super(Joint, self).__init__(**kwargs)
+        self.parent = kwargs.get('parent', '')
+        self.child = kwargs.get('child', '')
+        self.type = kwargs.get('type', '')
+        self.axis = kwargs.get('axis', Axis())
+
+        self.parent_link = None
+        self.child_link = None
+
+    def to_xml(self, fmt='sdf'):
+        '''returns a joint xml element'''
+        super(Joint, self).to_xml(fmt)
+
+        if self.parent_link.parent_joint:
+            self.urdf_pose = subtract_poses(self.global_pose,
+                                            self.parent_link.parent_joint.global_pose)
+        else:
+            self.urdf_pose = subtract_poses(self.global_pose, self.parent_link.global_pose)
+
+        joint = ET.Element('joint', {'name': self.name, 'type': self.type})
+        pose = self.pose if fmt=='sdf' else self.urdf_pose
+        joint.append(pose_to_xml(pose, fmt=fmt))
+
+        parent = ET.SubElement(joint, 'parent')
+        child = ET.SubElement(joint, 'child')
+        if fmt == 'sdf':
+            parent.text = self.parent
+            child.text = self.child
+            joint.append(self.axis.to_xml(fmt=fmt))
+        else:
+            parent.set('link', self.parent)
+            child.set('link', self.child)
+            joint.extend(self.axis.to_xml(fmt=fmt))
+
+        return joint
+

README.md

@@ -28,5 +28,42 @@ This workbench supports versions of FreeCAD>0.16.
 7. Save json
 8. Use the json with whatever you want. E.g. [`arbench_part_publisher`](https://github.com/mahaarbo/arbench_part_publisher)
 
-# Todo
- -[] Add export all parts to meshes
+
+# Freecad to Gazebo exporter
+
+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)
+```
+Note: Only links and joints are generated in the SDF model. To use the model with ros, use the URDF model.
+
+## 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.