From e28ff366743f864b13088c72105ab2d68481f58a Mon Sep 17 00:00:00 2001
From: Dawit Abate <dawitabate2@gmail.com>
Date: Fri, 5 Jul 2019 16:38:45 +0300
Subject: [PATCH] Added friction and damping support

---
 src/freecad_to_gazebo/freecad_exporter.py |  5 ++++-
 src/freecad_to_gazebo/model.py            | 13 ++++++++++++-
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/src/freecad_to_gazebo/freecad_exporter.py b/src/freecad_to_gazebo/freecad_exporter.py
index 49009ad..74e2ab6 100644
--- a/src/freecad_to_gazebo/freecad_exporter.py
+++ b/src/freecad_to_gazebo/freecad_exporter.py
@@ -38,6 +38,7 @@ def export_gazebo_model(assembly_file, model_dir, configs={}):
     model.sdf_version = '1.5'
 
     joint_limits = configs.get('joints_limits', {})
+    joint_dynamics = configs.get('joints_dynamics', {})
 
     constraints = []
     for obj in doc.Objects:
@@ -112,7 +113,9 @@ def export_gazebo_model(assembly_file, model_dir, configs={}):
                             lower_limit=joint_limits.get('lower', -90),
                             upper_limit=joint_limits.get('upper', 90),
                             effort_limit=joint_limits.get('effort', 10),
-                            velocity_limit=joint_limits.get('velocity', 10))
+                            velocity_limit=joint_limits.get('velocity', 10),
+                            friction=joint_dynamics.get('friction', 0),
+                            damping=joint_dynamics.get('damping', 0))
 
                 joint = Joint(name=parent.Label+'_'+child.Label,
                               pose=joint_pose,
diff --git a/src/freecad_to_gazebo/model.py b/src/freecad_to_gazebo/model.py
index 231f393..c7072a3 100644
--- a/src/freecad_to_gazebo/model.py
+++ b/src/freecad_to_gazebo/model.py
@@ -290,6 +290,8 @@ class Axis(SpatialEntity):
         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'):
@@ -310,6 +312,11 @@ class Axis(SpatialEntity):
             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:
@@ -321,7 +328,11 @@ class Axis(SpatialEntity):
             limit.set('effort', flt2str(self.effort_limit))
             limit.set('velocity', flt2str(self.velocity_limit))
 
-            return [axis, limit]
+            dynamics = ET.Element('dynamics')
+            dynamics.set('friction', flt2str(self.friction))
+            dynamics.set('damping', flt2str(self.damping))
+
+            return [axis, limit, dynamics]
         return axis