From a4f6cd7cb21e2db4ce09b6eda59b0480da43fc91 Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Wed, 30 Aug 2023 15:03:38 +0200
Subject: [PATCH 01/14] Add export to urdf of joint dynamics:damping and
 friction

---
 src/model/include/iDynTree/Model/FixedJoint.h |  8 ++++
 src/model/include/iDynTree/Model/IJoint.h     |  7 +++
 .../include/iDynTree/Model/PrismaticJoint.h   | 14 ++++++
 .../include/iDynTree/Model/RevoluteJoint.h    | 14 ++++++
 src/model/src/FixedJoint.cpp                  | 29 ++++++++++++
 src/model/src/PrismaticJoint.cpp              | 46 ++++++++++++++++++-
 src/model/src/RevoluteJoint.cpp               | 46 ++++++++++++++++++-
 .../codecs/include/private/JointElement.h     |  6 +++
 src/model_io/codecs/src/URDFModelExport.cpp   | 13 +++++-
 9 files changed, 180 insertions(+), 3 deletions(-)

diff --git a/src/model/include/iDynTree/Model/FixedJoint.h b/src/model/include/iDynTree/Model/FixedJoint.h
index cf53abf1d44..9ecfbacb79d 100644
--- a/src/model/include/iDynTree/Model/FixedJoint.h
+++ b/src/model/include/iDynTree/Model/FixedJoint.h
@@ -182,6 +182,14 @@ namespace iDynTree
         virtual double getMinPosLimit(const size_t _index) const;
         virtual double getMaxPosLimit(const size_t _index) const;
         virtual bool setPosLimits(const size_t _index, double & min, double & max);
+
+        // DYNAMICS METHODS
+        virtual bool hasDynamics() const;
+        virtual bool enableDynamics(const bool enable);
+        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const;
+        virtual double getDamping(const size_t _index) const;
+        virtual double getStaticFriction(const size_t _index) const;
+        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction);
     };
 }
 
diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h
index d3ee05948d6..774829966ee 100644
--- a/src/model/include/iDynTree/Model/IJoint.h
+++ b/src/model/include/iDynTree/Model/IJoint.h
@@ -363,6 +363,13 @@ namespace iDynTree
          */
         virtual bool setPosLimits(const size_t _index, double & min, double & max) = 0;
 
+        virtual bool hasDynamics() const = 0;
+        virtual bool enableDynamics(const bool enable) = 0;
+        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const = 0;
+        virtual double getDamping(const size_t _index) const = 0;
+        virtual double getStaticFriction(const size_t _index) const = 0;
+        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction) = 0;
+
         ///@}
     };
 
diff --git a/src/model/include/iDynTree/Model/PrismaticJoint.h b/src/model/include/iDynTree/Model/PrismaticJoint.h
index 042ec9f700a..2cce4f387ae 100644
--- a/src/model/include/iDynTree/Model/PrismaticJoint.h
+++ b/src/model/include/iDynTree/Model/PrismaticJoint.h
@@ -41,6 +41,12 @@ namespace iDynTree
         double m_minPos;
         double m_maxPos;
 
+        // Dynamic parameters
+        void disableDynamics();
+        bool m_has_dynamics;
+        double m_damping;
+        double m_static_friction;
+        
         // Cache attributes
         mutable double q_previous;
         mutable Transform link1_X_link2;
@@ -204,6 +210,14 @@ namespace iDynTree
         virtual double getMinPosLimit(const size_t _index) const;
         virtual double getMaxPosLimit(const size_t _index) const;
         virtual bool setPosLimits(const size_t _index, double & min, double & max);
+
+        // DYNAMICS METHODS
+        virtual bool hasDynamics() const;
+        virtual bool enableDynamics(const bool enable);
+        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const;
+        virtual double getDamping(const size_t _index) const;
+        virtual double getStaticFriction(const size_t _index) const;
+        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction);
     };
 }
 
diff --git a/src/model/include/iDynTree/Model/RevoluteJoint.h b/src/model/include/iDynTree/Model/RevoluteJoint.h
index 86248795116..d9a0dbe6bc8 100644
--- a/src/model/include/iDynTree/Model/RevoluteJoint.h
+++ b/src/model/include/iDynTree/Model/RevoluteJoint.h
@@ -41,6 +41,12 @@ namespace iDynTree
         double m_minPos;
         double m_maxPos;
 
+        // Dynamic parameters
+        void disableDynamics();
+        bool m_has_dynamics;
+        double m_damping;
+        double m_static_friction;
+
         // Cache attributes
         mutable double q_previous;
         mutable Transform link1_X_link2;
@@ -212,6 +218,14 @@ namespace iDynTree
         virtual double getMinPosLimit(const size_t _index) const;
         virtual double getMaxPosLimit(const size_t _index) const;
         virtual bool setPosLimits(const size_t _index, double & min, double & max);
+
+        // DYNAMICS METHODS
+        virtual bool hasDynamics() const;
+        virtual bool enableDynamics(const bool enable);
+        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const;
+        virtual double getDamping(const size_t _index) const;
+        virtual double getStaticFriction(const size_t _index) const;
+        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction);
     };
 }
 
diff --git a/src/model/src/FixedJoint.cpp b/src/model/src/FixedJoint.cpp
index ea857982332..92ea5ee2015 100644
--- a/src/model/src/FixedJoint.cpp
+++ b/src/model/src/FixedJoint.cpp
@@ -267,4 +267,33 @@ bool FixedJoint::setPosLimits(const size_t /*_index*/, double & /*min*/, double
     return false;
 }
 
+bool FixedJoint::getDynamicParameters(const size_t /*_index*/, double& /*damping*/, double& /*staticFriction*/) const
+{
+    return false;
+}
+
+bool FixedJoint::hasDynamics() const
+{
+    return false;
+}
+
+bool FixedJoint::enableDynamics(const bool /*enable*/)
+{
+    return false;
+}
+
+double FixedJoint::getDamping(const size_t _index) const
+{
+    return 0.0;
+}
+double FixedJoint::getStaticFriction(const size_t _index) const
+{
+    return 0.0;
+}
+
+bool FixedJoint::setDynamicParameters(const size_t /*_index*/, double& /*damping*/, double& /*staticFriction*/)
+{
+    return false;
+}
+
 }
diff --git a/src/model/src/PrismaticJoint.cpp b/src/model/src/PrismaticJoint.cpp
index bffd12f4683..d484e62910f 100644
--- a/src/model/src/PrismaticJoint.cpp
+++ b/src/model/src/PrismaticJoint.cpp
@@ -54,7 +54,9 @@ PrismaticJoint::PrismaticJoint(const PrismaticJoint& other):
                              link1_X_link2_at_rest(other.link1_X_link2_at_rest),
                              translation_axis_wrt_link1(other.translation_axis_wrt_link1),
                              m_hasPosLimits(other.m_hasPosLimits),
-                             m_minPos(other.m_minPos), m_maxPos(other.m_maxPos)
+                             m_minPos(other.m_minPos), m_maxPos(other.m_maxPos),
+                             m_has_dynamics(other.m_has_dynamics),
+                             m_damping(other.m_damping), m_static_friction(other.m_static_friction)
 {
     this->setPosCoordsOffset(other.getPosCoordsOffset());
     this->setDOFsOffset(other.getDOFsOffset());
@@ -383,6 +385,48 @@ bool PrismaticJoint::setPosLimits(const size_t /*_index*/, double & min, double
     return true;
 }
 
+bool PrismaticJoint::getDynamicParameters(const size_t /*_index*/, double& damping, double& staticFriction) const
+{
+    damping = m_damping;
+    staticFriction = m_static_friction;
+
+    return true;
+}
+
+void PrismaticJoint::disableDynamics()
+{
+    m_has_dynamics = false;
+    m_damping = .0;
+    m_static_friction = .0;
+}
+
+bool PrismaticJoint::hasDynamics() const
+{
+    return m_has_dynamics;
+}
+
+
+bool PrismaticJoint::enableDynamics(const bool enable)
+{
+    m_has_dynamics = enable;
+    return true;
+}
+
+double PrismaticJoint::getDamping(const size_t _index) const
+{
+    return m_damping;
+}
+double PrismaticJoint::getStaticFriction(const size_t _index) const
+{
+    return m_static_friction;
+}
+
+bool PrismaticJoint::setDynamicParameters(const size_t _index, double& damping, double& staticFriction)
+{
+    m_damping = damping;
+    m_static_friction = staticFriction;
 
+    return true;
+}
 
 }
diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp
index 1a30bfb531d..8d83314b186 100644
--- a/src/model/src/RevoluteJoint.cpp
+++ b/src/model/src/RevoluteJoint.cpp
@@ -66,7 +66,9 @@ RevoluteJoint::RevoluteJoint(const RevoluteJoint& other):
                              link1_X_link2_at_rest(other.link1_X_link2_at_rest),
                              rotation_axis_wrt_link1(other.rotation_axis_wrt_link1),
                              m_hasPosLimits(other.m_hasPosLimits),
-                             m_minPos(other.m_minPos), m_maxPos(other.m_maxPos)
+                             m_minPos(other.m_minPos), m_maxPos(other.m_maxPos),
+                            m_has_dynamics(other.m_has_dynamics),
+                            m_damping(other.m_damping), m_static_friction(other.m_static_friction)
 {
     this->setPosCoordsOffset(other.getPosCoordsOffset());
     this->setDOFsOffset(other.getDOFsOffset());
@@ -397,6 +399,48 @@ bool RevoluteJoint::setPosLimits(const size_t /*_index*/, double & min, double &
     return true;
 }
 
+bool RevoluteJoint::getDynamicParameters(const size_t /*_index*/, double& damping, double& staticFriction) const
+{
+    damping = m_damping;
+    staticFriction = m_static_friction;
+
+    return true;
+}
+
+void RevoluteJoint::disableDynamics()
+{
+    m_has_dynamics = false;
+    m_damping = .0;
+    m_static_friction = .0;
+}
+
+bool RevoluteJoint::hasDynamics() const
+{
+    return m_has_dynamics;
+}
+
+
+bool RevoluteJoint::enableDynamics(const bool enable)
+{
+    m_has_dynamics = enable;
+    return true;
+}
+
+double RevoluteJoint::getDamping(const size_t _index) const
+{
+    return m_damping;
+}
+double RevoluteJoint::getStaticFriction(const size_t _index) const
+{
+    return m_static_friction;
+}
+
+bool RevoluteJoint::setDynamicParameters(const size_t _index, double& damping, double& staticFriction)
+{
+    m_damping = damping;
+    m_static_friction = staticFriction;
 
+    return true;
+}
 
 }
diff --git a/src/model_io/codecs/include/private/JointElement.h b/src/model_io/codecs/include/private/JointElement.h
index f79a9fe2755..72950c0f82f 100644
--- a/src/model_io/codecs/include/private/JointElement.h
+++ b/src/model_io/codecs/include/private/JointElement.h
@@ -59,7 +59,13 @@ class iDynTree::JointElement : public iDynTree::XMLElement {
         double velocity;
     };
 
+    struct DyamicParams {
+        double damping;
+        double staticFriction;
+    };
+
     std::shared_ptr<Limits> m_limits;
+    std::shared_ptr<DyamicParams> m_dynamic_params;
     
 public:
     explicit JointElement(
diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp
index 8e63545a4bf..042a757ec48 100644
--- a/src/model_io/codecs/src/URDFModelExport.cpp
+++ b/src/model_io/codecs/src/URDFModelExport.cpp
@@ -405,7 +405,18 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
         xmlNewProp(limit_xml, BAD_CAST "velocity", BAD_CAST bufStr.c_str());
     }
 
-    // TODO(traversaro) : handle friction
+    if (joint->hasDynamics() && joint->getNrOfDOFs() == 1)
+    {
+
+        xmlNodePtr dynamics_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "dynamics", NULL);
+        std::string bufStr;
+        double damping, static_friction;
+        ok = ok && joint->getDynamicParameters(0, damping, static_friction);
+        ok = ok && doubleToStringWithClassicLocale(damping, bufStr);
+        xmlNewProp(dynamics_xml, BAD_CAST "damping", BAD_CAST bufStr.c_str());
+        ok = ok && doubleToStringWithClassicLocale(static_friction, bufStr);
+        xmlNewProp(dynamics_xml, BAD_CAST "friction", BAD_CAST bufStr.c_str());
+    }
 
     return ok;
 }

From 416dfd2bc46515acaf7863401652cdfef0a3625c Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Wed, 30 Aug 2023 15:17:08 +0200
Subject: [PATCH 02/14] Add reading of dynamics attributes of joint

---
 src/model_io/codecs/src/JointElement.cpp | 26 ++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp
index 0ce2470d49a..ab1beae9fec 100644
--- a/src/model_io/codecs/src/JointElement.cpp
+++ b/src/model_io/codecs/src/JointElement.cpp
@@ -132,6 +132,32 @@ namespace iDynTree {
                 return true;
             });
             return std::shared_ptr<XMLElement>(element);
+
+        } else if (name == "dynamics") {
+            m_dynamic_params = std::make_shared<DyamicParams>();
+            m_dynamic_params->damping = .0;
+            m_dynamic_params->staticFriction = .0;
+
+            // TODO: check how the defaults/required works
+            XMLElement* element = new XMLElement(getParserState(), name);
+            element->setAttributeCallback([this](const std::unordered_map<std::string, std::shared_ptr<XMLAttribute>>& attributes) {
+                auto found = attributes.find("damping");
+                if (found != attributes.end()) {
+                    double value = 0;
+                    if (stringToDoubleWithClassicLocale(found->second->value(), value)) {
+                        m_dynamic_params->damping = value;
+                    }
+                }
+                found = attributes.find("friction");
+                if (found != attributes.end()) {
+                    double value = 0;
+                    if (stringToDoubleWithClassicLocale(found->second->value(), value)) {
+                        m_dynamic_params->staticFriction = value;
+                    }
+                }
+                return true;
+                });
+                return std::shared_ptr<XMLElement>(element);
         }
         return std::make_shared<XMLElement>(getParserState(), name);
     }

From 6816a3f1cfab6ff270cfebb81f37822c95e5f13b Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Wed, 30 Aug 2023 15:33:20 +0200
Subject: [PATCH 03/14] Add comments to dyn params

---
 src/model/include/iDynTree/Model/IJoint.h | 47 +++++++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h
index 774829966ee..2c6e43d489b 100644
--- a/src/model/include/iDynTree/Model/IJoint.h
+++ b/src/model/include/iDynTree/Model/IJoint.h
@@ -363,11 +363,58 @@ namespace iDynTree
          */
         virtual bool setPosLimits(const size_t _index, double & min, double & max) = 0;
 
+        /**
+         * @name Dynamic parameters handling methods.
+         *  Methods for handling dynamic parameters of joints.
+         *
+         *  A joint can have dynamic parameters or not. 
+         *  Supported parameters are damping coefficient and static friction coefficient.
+         */
+        ///@{
+        /**
+         * Method to check if the joint has dynamic parameters.
+         *
+         * @return true if the joints has dynamic parameters
+         */
         virtual bool hasDynamics() const = 0;
+         
+         /**
+         * Method to enable the joint dynamics.
+         *
+         * @return true if everything went correctly, false otherwise
+         */
         virtual bool enableDynamics(const bool enable) = 0;
+
+         /**
+         * Get damping and static friction parameters of the joint, for the _index dof.
+         * The damping coefficient is expressed in N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint.
+         * The static friction is expressed in N for a prismatic joint, N∙m for a revolute joint.
+         * 
+         * @param[in] _index index of the dof for which the dynamic parameters are obtained.
+         * @return true if everything is correct, false otherwise.
+         */
         virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const = 0;
+
+        /**
+         * Get the damping coefficient of the joint, bindings-friendly version.
+         * The unit is N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint.
+         */
         virtual double getDamping(const size_t _index) const = 0;
+
+        /**
+         * Get the damping coefficient of the joint, bindings-friendly version.
+         * The unit is N for a prismatic joint, N∙m for a revolute joint.
+         */
         virtual double getStaticFriction(const size_t _index) const = 0;
+
+        /**
+         * Set the dynamic parameters for a dof the joint.
+         * The damping coefficient is expressed in N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint.
+         * The static friction is expressed in N for a prismatic joint, N∙m for a revolute joint.
+         * 
+         * @note This just sets the internal damping and static friction of the joint.
+         *       To set them as enabled, you need to call the enableDynamics(true) method.
+         */
         virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction) = 0;
 
         ///@}

From 8dfc3f99532003f2152390ac514a495871f8c2a8 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Wed, 30 Aug 2023 17:45:54 +0200
Subject: [PATCH 04/14] Generalize joint dynamics interface

---
 src/model/include/iDynTree/Model/IJoint.h | 87 +++++++++++++++++------
 1 file changed, 64 insertions(+), 23 deletions(-)

diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h
index 2c6e43d489b..b6b9df0fc56 100644
--- a/src/model/include/iDynTree/Model/IJoint.h
+++ b/src/model/include/iDynTree/Model/IJoint.h
@@ -25,6 +25,24 @@ namespace iDynTree
     class VectorDynSize;
     class SpatialMotionVector;
 
+    enum JointDynamicsType
+    {
+        /**
+         * NoDynamics: No joint dynamics is assumed for the joint.
+         * This joint dynamics type does not consider any parameter.
+         */
+        NoJointDynamics = 0,
+
+        /**
+         * URDFJointDynamics: Dynamics described by the URDF 1.0 specification.
+         * 
+         * This joint dynamics type consider the following parameters:
+         *  * `Damping`
+         *  * `StaticFriction`
+         */
+        URDFJointDynamics = 1    
+    };
+
     /**
      * Interface (i.e. abstract class) exposed by classes that implement a Joint.
      * A Joint is the basic representation of the motion allowed between two links.
@@ -364,58 +382,81 @@ namespace iDynTree
         virtual bool setPosLimits(const size_t _index, double & min, double & max) = 0;
 
         /**
-         * @name Dynamic parameters handling methods.
-         *  Methods for handling dynamic parameters of joints.
+         * @name Joint dynamics methods.
+         * 
+         *  Methods for handling representation of joint dynamics. 
+         *  The precise definition of "joint dynamics" is not precisely, as depending on the
+         *  specific application the kind of joint dynamics model can be different, and in some
+         *  case it may be even just instantaneous models (for example, when only the damping is considered).
          *
-         *  A joint can have dynamic parameters or not. 
-         *  Supported parameters are damping coefficient and static friction coefficient.
+         *  For the type of joint dynamics supported, see the iDynTree::JointDynamicsType enum documentation.
+         *  
+         *  The joint dynamics model are used in the following contexts:
+         *   * In methods to serialize and deserialize URDF files
+         * 
+         *  The joint dynamics are **not used at all** in classes to compute kinematics and dynamics quantities,
+         *  such as iDynTree::KinDynComputations .
          */
         ///@{
+
         /**
-         * Method to check if the joint has dynamic parameters.
+         * Method to get the specific joint dynamics type used for the joint.
+         * \note: It is assume that all the degrees of freedom of a joint share the same joint dynamics type.
          *
-         * @return true if the joints has dynamic parameters
+         * @return the specific joint dynamics type used for the joint.
          */
-        virtual bool hasDynamics() const = 0;
+        virtual JointDynamicsType getJointDynamicsType() const = 0;
          
          /**
-         * Method to enable the joint dynamics.
+         * Method to get the specific joint dynamics type used for the joint.
+         * \note: It is assume that all the degrees of freedom of a joint share the same joint dynamics type.
          *
          * @return true if everything went correctly, false otherwise
          */
-        virtual bool enableDynamics(const bool enable) = 0;
+        virtual bool setJointDynamicsType(const JointDynamicsType enable) = 0;
 
          /**
-         * Get damping and static friction parameters of the joint, for the _index dof.
+         * Set damping parameter of the joint, for the _index dof.
          * The damping coefficient is expressed in N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint.
-         * The static friction is expressed in N for a prismatic joint, N∙m for a revolute joint.
          * 
+         * This parameter is considered in the following joint dynamics types:
+         * * `URDFJointDynamics`
+         *
+         * @param[in] _index index of the dof for which the dynamic parameters are obtained.
+         * @return true if everything is correct, false otherwise.
+         */
+        virtual bool setDamping(const size_t _index, double& damping) const = 0;
+
+         /**
+         * Set static friction parameter of the joint, for the _index dof.
+         * The static friction coefficient is expressed in N for a prismatic joint, N∙m for a revolute joint.
+         * 
+         * This parameter is considered in the following joint dynamics types:
+         * * `URDFJointDynamics`
+         *
          * @param[in] _index index of the dof for which the dynamic parameters are obtained.
          * @return true if everything is correct, false otherwise.
          */
-        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const = 0;
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction) const = 0;
 
         /**
-         * Get the damping coefficient of the joint, bindings-friendly version.
+         * Get the damping coefficient of the joint.
          * The unit is N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint.
+         *
+         * This parameter is considered in the following joint dynamics types:
+         * * `URDFJointDynamics`
          */
         virtual double getDamping(const size_t _index) const = 0;
 
         /**
-         * Get the damping coefficient of the joint, bindings-friendly version.
+         * Get the static friction coefficient of the joint.
          * The unit is N for a prismatic joint, N∙m for a revolute joint.
+         * 
+         * This parameter is considered in the following joint dynamics types:
+         * * `URDFJointDynamics`
          */
         virtual double getStaticFriction(const size_t _index) const = 0;
 
-        /**
-         * Set the dynamic parameters for a dof the joint.
-         * The damping coefficient is expressed in N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint.
-         * The static friction is expressed in N for a prismatic joint, N∙m for a revolute joint.
-         * 
-         * @note This just sets the internal damping and static friction of the joint.
-         *       To set them as enabled, you need to call the enableDynamics(true) method.
-         */
-        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction) = 0;
 
         ///@}
     };

From 1df22d4a6c392578002aaaff32647ddf69fa1bb7 Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Wed, 30 Aug 2023 18:00:42 +0200
Subject: [PATCH 05/14] initialize dyn params in constructors

---
 src/model/src/PrismaticJoint.cpp | 3 +++
 src/model/src/RevoluteJoint.cpp  | 3 +++
 2 files changed, 6 insertions(+)

diff --git a/src/model/src/PrismaticJoint.cpp b/src/model/src/PrismaticJoint.cpp
index d484e62910f..b2a4722e9a5 100644
--- a/src/model/src/PrismaticJoint.cpp
+++ b/src/model/src/PrismaticJoint.cpp
@@ -34,6 +34,8 @@ PrismaticJoint::PrismaticJoint():
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->disableDynamics();
+
 }
 
 PrismaticJoint::PrismaticJoint(const LinkIndex _link1, const LinkIndex _link2,
@@ -47,6 +49,7 @@ PrismaticJoint::PrismaticJoint(const LinkIndex _link1, const LinkIndex _link2,
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->disableDynamics();
 }
 
 PrismaticJoint::PrismaticJoint(const PrismaticJoint& other):
diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp
index 8d83314b186..16962cfc4b0 100644
--- a/src/model/src/RevoluteJoint.cpp
+++ b/src/model/src/RevoluteJoint.cpp
@@ -34,6 +34,7 @@ RevoluteJoint::RevoluteJoint():
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->disableDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const LinkIndex _link1, const LinkIndex _link2,
@@ -47,6 +48,7 @@ RevoluteJoint::RevoluteJoint(const LinkIndex _link1, const LinkIndex _link2,
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->disableDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const Transform& _link1_X_link2, const Axis& _rotation_axis_wrt_link1):
@@ -59,6 +61,7 @@ RevoluteJoint::RevoluteJoint(const Transform& _link1_X_link2, const Axis& _rotat
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->disableDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const RevoluteJoint& other):

From 5a92e8a28d3d8b609cbe66eaad87cc20c96101b9 Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Thu, 31 Aug 2023 10:26:31 +0200
Subject: [PATCH 06/14] use jointdynamicstype

---
 src/model/include/iDynTree/Model/FixedJoint.h |  8 ++--
 src/model/include/iDynTree/Model/IJoint.h     |  4 +-
 .../include/iDynTree/Model/PrismaticJoint.h   | 12 +++---
 .../include/iDynTree/Model/RevoluteJoint.h    | 12 +++---
 src/model/src/FixedJoint.cpp                  | 18 ++++----
 src/model/src/PrismaticJoint.cpp              | 40 ++++++++----------
 src/model/src/RevoluteJoint.cpp               | 41 +++++++++----------
 .../codecs/include/private/JointElement.h     |  1 +
 src/model_io/codecs/src/JointElement.cpp      |  1 +
 src/model_io/codecs/src/URDFModelExport.cpp   |  7 ++--
 10 files changed, 70 insertions(+), 74 deletions(-)

diff --git a/src/model/include/iDynTree/Model/FixedJoint.h b/src/model/include/iDynTree/Model/FixedJoint.h
index 9ecfbacb79d..5b9da0e297e 100644
--- a/src/model/include/iDynTree/Model/FixedJoint.h
+++ b/src/model/include/iDynTree/Model/FixedJoint.h
@@ -184,12 +184,12 @@ namespace iDynTree
         virtual bool setPosLimits(const size_t _index, double & min, double & max);
 
         // DYNAMICS METHODS
-        virtual bool hasDynamics() const;
-        virtual bool enableDynamics(const bool enable);
-        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const;
+        virtual JointDynamicsType getJointDynamicsType() const;
+        virtual bool setJointDynamicsType(const JointDynamicsType enable);
         virtual double getDamping(const size_t _index) const;
         virtual double getStaticFriction(const size_t _index) const;
-        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction);
+        virtual bool setDamping(const size_t _index, double& damping);
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction);
     };
 }
 
diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h
index b6b9df0fc56..8a28e0e95f5 100644
--- a/src/model/include/iDynTree/Model/IJoint.h
+++ b/src/model/include/iDynTree/Model/IJoint.h
@@ -425,7 +425,7 @@ namespace iDynTree
          * @param[in] _index index of the dof for which the dynamic parameters are obtained.
          * @return true if everything is correct, false otherwise.
          */
-        virtual bool setDamping(const size_t _index, double& damping) const = 0;
+        virtual bool setDamping(const size_t _index, double& damping) = 0;
 
          /**
          * Set static friction parameter of the joint, for the _index dof.
@@ -437,7 +437,7 @@ namespace iDynTree
          * @param[in] _index index of the dof for which the dynamic parameters are obtained.
          * @return true if everything is correct, false otherwise.
          */
-        virtual bool setStaticFriction(const size_t _index, double& staticFriction) const = 0;
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction) = 0;
 
         /**
          * Get the damping coefficient of the joint.
diff --git a/src/model/include/iDynTree/Model/PrismaticJoint.h b/src/model/include/iDynTree/Model/PrismaticJoint.h
index 2cce4f387ae..8230c113432 100644
--- a/src/model/include/iDynTree/Model/PrismaticJoint.h
+++ b/src/model/include/iDynTree/Model/PrismaticJoint.h
@@ -42,8 +42,8 @@ namespace iDynTree
         double m_maxPos;
 
         // Dynamic parameters
-        void disableDynamics();
-        bool m_has_dynamics;
+        void resetJointDynamics();
+        JointDynamicsType m_joint_dynamics_type;
         double m_damping;
         double m_static_friction;
         
@@ -212,12 +212,12 @@ namespace iDynTree
         virtual bool setPosLimits(const size_t _index, double & min, double & max);
 
         // DYNAMICS METHODS
-        virtual bool hasDynamics() const;
-        virtual bool enableDynamics(const bool enable);
-        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const;
+        virtual JointDynamicsType getJointDynamicsType() const;
+        virtual bool setJointDynamicsType(const JointDynamicsType enable);
         virtual double getDamping(const size_t _index) const;
         virtual double getStaticFriction(const size_t _index) const;
-        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction);
+        virtual bool setDamping(const size_t _index, double& damping);
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction);
     };
 }
 
diff --git a/src/model/include/iDynTree/Model/RevoluteJoint.h b/src/model/include/iDynTree/Model/RevoluteJoint.h
index d9a0dbe6bc8..ad1fab38e6f 100644
--- a/src/model/include/iDynTree/Model/RevoluteJoint.h
+++ b/src/model/include/iDynTree/Model/RevoluteJoint.h
@@ -42,8 +42,8 @@ namespace iDynTree
         double m_maxPos;
 
         // Dynamic parameters
-        void disableDynamics();
-        bool m_has_dynamics;
+        void resetJointDynamics();
+        JointDynamicsType m_joint_dynamics_type;
         double m_damping;
         double m_static_friction;
 
@@ -220,12 +220,12 @@ namespace iDynTree
         virtual bool setPosLimits(const size_t _index, double & min, double & max);
 
         // DYNAMICS METHODS
-        virtual bool hasDynamics() const;
-        virtual bool enableDynamics(const bool enable);
-        virtual bool getDynamicParameters(const size_t _index, double& damping, double& staticFriction) const;
+        virtual JointDynamicsType getJointDynamicsType() const;
+        virtual bool setJointDynamicsType(const JointDynamicsType enable);
         virtual double getDamping(const size_t _index) const;
         virtual double getStaticFriction(const size_t _index) const;
-        virtual bool setDynamicParameters(const size_t _index, double& damping, double& staticFriction);
+        virtual bool setDamping(const size_t _index, double& damping);
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction);
     };
 }
 
diff --git a/src/model/src/FixedJoint.cpp b/src/model/src/FixedJoint.cpp
index 92ea5ee2015..6d07a409532 100644
--- a/src/model/src/FixedJoint.cpp
+++ b/src/model/src/FixedJoint.cpp
@@ -267,17 +267,12 @@ bool FixedJoint::setPosLimits(const size_t /*_index*/, double & /*min*/, double
     return false;
 }
 
-bool FixedJoint::getDynamicParameters(const size_t /*_index*/, double& /*damping*/, double& /*staticFriction*/) const
+JointDynamicsType FixedJoint::getJointDynamicsType() const
 {
-    return false;
-}
-
-bool FixedJoint::hasDynamics() const
-{
-    return false;
+    return NoJointDynamics;
 }
 
-bool FixedJoint::enableDynamics(const bool /*enable*/)
+bool FixedJoint::setJointDynamicsType(const JointDynamicsType enable)
 {
     return false;
 }
@@ -291,7 +286,12 @@ double FixedJoint::getStaticFriction(const size_t _index) const
     return 0.0;
 }
 
-bool FixedJoint::setDynamicParameters(const size_t /*_index*/, double& /*damping*/, double& /*staticFriction*/)
+bool FixedJoint::setDamping(const size_t /*_index*/, double& /*damping*/)
+{
+    return false;
+}
+
+bool FixedJoint::setStaticFriction(const size_t /*_index*/, double& /*staticFriction*/)
 {
     return false;
 }
diff --git a/src/model/src/PrismaticJoint.cpp b/src/model/src/PrismaticJoint.cpp
index b2a4722e9a5..75cc135e25f 100644
--- a/src/model/src/PrismaticJoint.cpp
+++ b/src/model/src/PrismaticJoint.cpp
@@ -34,8 +34,7 @@ PrismaticJoint::PrismaticJoint():
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
-    this->disableDynamics();
-
+    this->resetJointDynamics();
 }
 
 PrismaticJoint::PrismaticJoint(const LinkIndex _link1, const LinkIndex _link2,
@@ -49,7 +48,7 @@ PrismaticJoint::PrismaticJoint(const LinkIndex _link1, const LinkIndex _link2,
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
-    this->disableDynamics();
+    this->resetJointDynamics();
 }
 
 PrismaticJoint::PrismaticJoint(const PrismaticJoint& other):
@@ -58,7 +57,7 @@ PrismaticJoint::PrismaticJoint(const PrismaticJoint& other):
                              translation_axis_wrt_link1(other.translation_axis_wrt_link1),
                              m_hasPosLimits(other.m_hasPosLimits),
                              m_minPos(other.m_minPos), m_maxPos(other.m_maxPos),
-                             m_has_dynamics(other.m_has_dynamics),
+                             m_joint_dynamics_type(other.m_joint_dynamics_type),
                              m_damping(other.m_damping), m_static_friction(other.m_static_friction)
 {
     this->setPosCoordsOffset(other.getPosCoordsOffset());
@@ -388,30 +387,21 @@ bool PrismaticJoint::setPosLimits(const size_t /*_index*/, double & min, double
     return true;
 }
 
-bool PrismaticJoint::getDynamicParameters(const size_t /*_index*/, double& damping, double& staticFriction) const
-{
-    damping = m_damping;
-    staticFriction = m_static_friction;
-
-    return true;
-}
-
-void PrismaticJoint::disableDynamics()
+void PrismaticJoint::resetJointDynamics()
 {
-    m_has_dynamics = false;
-    m_damping = .0;
-    m_static_friction = .0;
+    m_joint_dynamics_type = NoJointDynamics;
+    m_damping = 0.0;
+    m_static_friction = 0.0;
 }
 
-bool PrismaticJoint::hasDynamics() const
+JointDynamicsType PrismaticJoint::getJointDynamicsType() const
 {
-    return m_has_dynamics;
+    return m_joint_dynamics_type;
 }
 
-
-bool PrismaticJoint::enableDynamics(const bool enable)
+bool PrismaticJoint::setJointDynamicsType(const JointDynamicsType enable)
 {
-    m_has_dynamics = enable;
+    m_joint_dynamics_type = enable;
     return true;
 }
 
@@ -424,9 +414,15 @@ double PrismaticJoint::getStaticFriction(const size_t _index) const
     return m_static_friction;
 }
 
-bool PrismaticJoint::setDynamicParameters(const size_t _index, double& damping, double& staticFriction)
+bool PrismaticJoint::setDamping(const size_t _index, double& damping)
 {
     m_damping = damping;
+
+    return true;
+}
+
+bool PrismaticJoint::setStaticFriction(const size_t _index, double& staticFriction)
+{
     m_static_friction = staticFriction;
 
     return true;
diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp
index 16962cfc4b0..1614720c053 100644
--- a/src/model/src/RevoluteJoint.cpp
+++ b/src/model/src/RevoluteJoint.cpp
@@ -34,7 +34,6 @@ RevoluteJoint::RevoluteJoint():
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
-    this->disableDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const LinkIndex _link1, const LinkIndex _link2,
@@ -48,7 +47,7 @@ RevoluteJoint::RevoluteJoint(const LinkIndex _link1, const LinkIndex _link2,
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
-    this->disableDynamics();
+    this->resetJointDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const Transform& _link1_X_link2, const Axis& _rotation_axis_wrt_link1):
@@ -61,7 +60,7 @@ RevoluteJoint::RevoluteJoint(const Transform& _link1_X_link2, const Axis& _rotat
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
-    this->disableDynamics();
+    this->resetJointDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const RevoluteJoint& other):
@@ -70,7 +69,7 @@ RevoluteJoint::RevoluteJoint(const RevoluteJoint& other):
                              rotation_axis_wrt_link1(other.rotation_axis_wrt_link1),
                              m_hasPosLimits(other.m_hasPosLimits),
                              m_minPos(other.m_minPos), m_maxPos(other.m_maxPos),
-                            m_has_dynamics(other.m_has_dynamics),
+                            m_joint_dynamics_type(other.m_joint_dynamics_type),
                             m_damping(other.m_damping), m_static_friction(other.m_static_friction)
 {
     this->setPosCoordsOffset(other.getPosCoordsOffset());
@@ -402,30 +401,21 @@ bool RevoluteJoint::setPosLimits(const size_t /*_index*/, double & min, double &
     return true;
 }
 
-bool RevoluteJoint::getDynamicParameters(const size_t /*_index*/, double& damping, double& staticFriction) const
+void RevoluteJoint::resetJointDynamics()
 {
-    damping = m_damping;
-    staticFriction = m_static_friction;
-
-    return true;
-}
-
-void RevoluteJoint::disableDynamics()
-{
-    m_has_dynamics = false;
-    m_damping = .0;
-    m_static_friction = .0;
+    m_joint_dynamics_type = NoJointDynamics;
+    m_damping = 0.0;
+    m_static_friction = 0.0;
 }
 
-bool RevoluteJoint::hasDynamics() const
+JointDynamicsType RevoluteJoint::getJointDynamicsType() const
 {
-    return m_has_dynamics;
+    return m_joint_dynamics_type;
 }
 
-
-bool RevoluteJoint::enableDynamics(const bool enable)
+bool RevoluteJoint::setJointDynamicsType(const JointDynamicsType enable)
 {
-    m_has_dynamics = enable;
+    m_joint_dynamics_type = enable;
     return true;
 }
 
@@ -433,14 +423,21 @@ double RevoluteJoint::getDamping(const size_t _index) const
 {
     return m_damping;
 }
+
 double RevoluteJoint::getStaticFriction(const size_t _index) const
 {
     return m_static_friction;
 }
 
-bool RevoluteJoint::setDynamicParameters(const size_t _index, double& damping, double& staticFriction)
+bool RevoluteJoint::setDamping(const size_t _index, double& damping)
 {
     m_damping = damping;
+
+    return true;
+}
+
+bool RevoluteJoint::setStaticFriction(const size_t _index, double& staticFriction)
+{
     m_static_friction = staticFriction;
 
     return true;
diff --git a/src/model_io/codecs/include/private/JointElement.h b/src/model_io/codecs/include/private/JointElement.h
index 72950c0f82f..733b6bffebd 100644
--- a/src/model_io/codecs/include/private/JointElement.h
+++ b/src/model_io/codecs/include/private/JointElement.h
@@ -60,6 +60,7 @@ class iDynTree::JointElement : public iDynTree::XMLElement {
     };
 
     struct DyamicParams {
+        uint8_t jointDynamicsType;
         double damping;
         double staticFriction;
     };
diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp
index ab1beae9fec..64d351d1a43 100644
--- a/src/model_io/codecs/src/JointElement.cpp
+++ b/src/model_io/codecs/src/JointElement.cpp
@@ -135,6 +135,7 @@ namespace iDynTree {
 
         } else if (name == "dynamics") {
             m_dynamic_params = std::make_shared<DyamicParams>();
+            m_dynamic_params->jointDynamicsType = NoJointDynamics;
             m_dynamic_params->damping = .0;
             m_dynamic_params->staticFriction = .0;
 
diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp
index 042a757ec48..460630230b2 100644
--- a/src/model_io/codecs/src/URDFModelExport.cpp
+++ b/src/model_io/codecs/src/URDFModelExport.cpp
@@ -405,13 +405,14 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
         xmlNewProp(limit_xml, BAD_CAST "velocity", BAD_CAST bufStr.c_str());
     }
 
-    if (joint->hasDynamics() && joint->getNrOfDOFs() == 1)
+    if (joint->getJointDynamicsType() != NoJointDynamics && joint->getNrOfDOFs() == 1)
     {
 
         xmlNodePtr dynamics_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "dynamics", NULL);
         std::string bufStr;
-        double damping, static_friction;
-        ok = ok && joint->getDynamicParameters(0, damping, static_friction);
+        double damping = 0.0, static_friction = 0.0;
+        damping = joint->getDamping(0);
+        static_friction = joint->getStaticFriction(0);
         ok = ok && doubleToStringWithClassicLocale(damping, bufStr);
         xmlNewProp(dynamics_xml, BAD_CAST "damping", BAD_CAST bufStr.c_str());
         ok = ok && doubleToStringWithClassicLocale(static_friction, bufStr);

From e947ae1a1e24929dd7c13a259b0f44aebb08dded Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Thu, 31 Aug 2023 15:11:36 +0200
Subject: [PATCH 07/14] additional fix in modelio to reduce memory leaks

---
 bindings/pybind11/idyntree_model.cpp     |  8 +++++++-
 src/model_io/codecs/src/JointElement.cpp | 11 ++++++++++-
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp
index 65d47fa0846..ae423f064ca 100644
--- a/bindings/pybind11/idyntree_model.cpp
+++ b/bindings/pybind11/idyntree_model.cpp
@@ -95,7 +95,13 @@ void jointClassDefinition(py::class_<IJoint>& joint) {
       .def("has_pos_limits", &IJoint::hasPosLimits)
       .def("set_pos_limits", &IJoint::setPosLimits)
       .def("get_max_pos_limit", &IJoint::getMaxPosLimit)
-      .def("get_min_pos_limit", &IJoint::getMinPosLimit);
+      .def("get_min_pos_limit", &IJoint::getMinPosLimit)
+      .def("get_joint_dynamics_type", &IJoint::getJointDynamicsType)
+      .def("get_damping", &IJoint::getDamping)
+      .def("get_static_friction", &IJoint::getStaticFriction)
+      .def("set_joint_dynamics_type", &IJoint::setJointDynamicsType)
+      .def("set_damping", &IJoint::setDamping)
+      .def("set_static_friction", &IJoint::setStaticFriction);
 }
 
 void traversalClassDefinition(py::class_<Traversal>& traversal) {
diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp
index 64d351d1a43..1fca4f0328a 100644
--- a/src/model_io/codecs/src/JointElement.cpp
+++ b/src/model_io/codecs/src/JointElement.cpp
@@ -135,7 +135,7 @@ namespace iDynTree {
 
         } else if (name == "dynamics") {
             m_dynamic_params = std::make_shared<DyamicParams>();
-            m_dynamic_params->jointDynamicsType = NoJointDynamics;
+            m_dynamic_params->jointDynamicsType = URDFJointDynamics;
             m_dynamic_params->damping = .0;
             m_dynamic_params->staticFriction = .0;
 
@@ -202,6 +202,15 @@ namespace iDynTree {
                 std::string errStr = "Joint " + m_jointName + " misses the limit tag.";
                 reportWarning("JointElement", "", errStr.c_str());
             }
+
+            if(m_dynamic_params) {
+                info.joint->setJointDynamicsType(URDFJointDynamics);
+                info.joint->setDamping(0, m_dynamic_params->damping);
+                info.joint->setStaticFriction(0, m_dynamic_params->staticFriction);
+            } else if (m_jointType == "revolute" || m_jointType == "prismatic") {
+                std::string errStr = "Joint " + m_jointName + " misses the dynamics tag.";
+                reportWarning("JointElement", "", errStr.c_str());
+            }
             
             if (!map->insert(std::unordered_map<std::string, JointElement::JointInfo>::value_type(m_jointName, info)).second) {
                 std::string errStr = "Duplicate joint " + m_jointName + " found.";

From 0afa7631316a36984622c310d69cc5db05cda476 Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Thu, 31 Aug 2023 15:38:05 +0200
Subject: [PATCH 08/14] remove warning if dynamics is missing

---
 src/model_io/codecs/src/JointElement.cpp | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp
index 1fca4f0328a..e904a27685f 100644
--- a/src/model_io/codecs/src/JointElement.cpp
+++ b/src/model_io/codecs/src/JointElement.cpp
@@ -207,9 +207,6 @@ namespace iDynTree {
                 info.joint->setJointDynamicsType(URDFJointDynamics);
                 info.joint->setDamping(0, m_dynamic_params->damping);
                 info.joint->setStaticFriction(0, m_dynamic_params->staticFriction);
-            } else if (m_jointType == "revolute" || m_jointType == "prismatic") {
-                std::string errStr = "Joint " + m_jointName + " misses the dynamics tag.";
-                reportWarning("JointElement", "", errStr.c_str());
             }
             
             if (!map->insert(std::unordered_map<std::string, JointElement::JointInfo>::value_type(m_jointName, info)).second) {

From 8425b4e4cded395bde560bb3218f7ea944e72f03 Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Thu, 31 Aug 2023 15:40:00 +0200
Subject: [PATCH 09/14] invert check on dynamics

---
 src/model_io/codecs/src/URDFModelExport.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp
index 460630230b2..03ab7f17a35 100644
--- a/src/model_io/codecs/src/URDFModelExport.cpp
+++ b/src/model_io/codecs/src/URDFModelExport.cpp
@@ -405,7 +405,7 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
         xmlNewProp(limit_xml, BAD_CAST "velocity", BAD_CAST bufStr.c_str());
     }
 
-    if (joint->getJointDynamicsType() != NoJointDynamics && joint->getNrOfDOFs() == 1)
+    if (joint->getJointDynamicsType() == URDFJointDynamics && joint->getNrOfDOFs() == 1)
     {
 
         xmlNodePtr dynamics_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "dynamics", NULL);

From c57b3865e9d4d73fddbe8a08067770cfdc1980c9 Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Thu, 31 Aug 2023 16:19:45 +0200
Subject: [PATCH 10/14] Add additional reset of jdyn to avoid uninitialized
 variables

---
 src/model/src/RevoluteJoint.cpp               |  1 +
 .../codecs/include/private/JointElement.h     |  8 ++--
 src/model_io/codecs/src/JointElement.cpp      | 39 ++++++++++---------
 src/model_io/codecs/src/URDFModelExport.cpp   |  1 -
 .../codecs/tests/ModelExporterUnitTest.cpp    |  1 +
 5 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp
index 1614720c053..f4af413a2d2 100644
--- a/src/model/src/RevoluteJoint.cpp
+++ b/src/model/src/RevoluteJoint.cpp
@@ -34,6 +34,7 @@ RevoluteJoint::RevoluteJoint():
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->resetJointDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const LinkIndex _link1, const LinkIndex _link2,
diff --git a/src/model_io/codecs/include/private/JointElement.h b/src/model_io/codecs/include/private/JointElement.h
index 733b6bffebd..379043d9881 100644
--- a/src/model_io/codecs/include/private/JointElement.h
+++ b/src/model_io/codecs/include/private/JointElement.h
@@ -15,6 +15,8 @@
 
 #include <iDynTree/XMLElement.h>
 
+#include <iDynTree/Model/IJoint.h>
+
 #include <iDynTree/Core/Axis.h>
 #include <iDynTree/Core/Transform.h>
 
@@ -59,14 +61,14 @@ class iDynTree::JointElement : public iDynTree::XMLElement {
         double velocity;
     };
 
-    struct DyamicParams {
-        uint8_t jointDynamicsType;
+    struct JointDyamicsParams {
+        JointDynamicsType jointDynamicsType;
         double damping;
         double staticFriction;
     };
 
     std::shared_ptr<Limits> m_limits;
-    std::shared_ptr<DyamicParams> m_dynamic_params;
+    std::shared_ptr<JointDyamicsParams> m_dynamic_params;
     
 public:
     explicit JointElement(
diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp
index e904a27685f..952753ae437 100644
--- a/src/model_io/codecs/src/JointElement.cpp
+++ b/src/model_io/codecs/src/JointElement.cpp
@@ -18,7 +18,6 @@
 #include <iDynTree/XMLAttribute.h>
 #include <iDynTree/XMLParser.h>
 
-#include <iDynTree/Model/IJoint.h>
 #include <iDynTree/Model/FixedJoint.h>
 #include <iDynTree/Model/PrismaticJoint.h>
 #include <iDynTree/Model/RevoluteJoint.h>
@@ -134,31 +133,33 @@ namespace iDynTree {
             return std::shared_ptr<XMLElement>(element);
 
         } else if (name == "dynamics") {
-            m_dynamic_params = std::make_shared<DyamicParams>();
+            m_dynamic_params = std::make_shared<JointDyamicsParams>();
             m_dynamic_params->jointDynamicsType = URDFJointDynamics;
             m_dynamic_params->damping = .0;
             m_dynamic_params->staticFriction = .0;
 
             // TODO: check how the defaults/required works
             XMLElement* element = new XMLElement(getParserState(), name);
-            element->setAttributeCallback([this](const std::unordered_map<std::string, std::shared_ptr<XMLAttribute>>& attributes) {
-                auto found = attributes.find("damping");
-                if (found != attributes.end()) {
-                    double value = 0;
-                    if (stringToDoubleWithClassicLocale(found->second->value(), value)) {
-                        m_dynamic_params->damping = value;
+            element->setAttributeCallback(
+                [this](const std::unordered_map<std::string, std::shared_ptr<XMLAttribute>>& attributes) {
+                    auto found = attributes.find("damping");
+                    if (found != attributes.end()) {
+                        double value = 0;
+                        if (stringToDoubleWithClassicLocale(found->second->value(), value)) {
+                            m_dynamic_params->damping = value;
+                        }
                     }
-                }
-                found = attributes.find("friction");
-                if (found != attributes.end()) {
-                    double value = 0;
-                    if (stringToDoubleWithClassicLocale(found->second->value(), value)) {
-                        m_dynamic_params->staticFriction = value;
+                    found = attributes.find("friction");
+                    if (found != attributes.end()) {
+                        double value = 0;
+                        if (stringToDoubleWithClassicLocale(found->second->value(), value)) {
+                            m_dynamic_params->staticFriction = value;
+                        }
                     }
-                }
-                return true;
-                });
-                return std::shared_ptr<XMLElement>(element);
+                    return true;
+                    }
+                );
+            return std::shared_ptr<XMLElement>(element);
         }
         return std::make_shared<XMLElement>(getParserState(), name);
     }
@@ -208,7 +209,7 @@ namespace iDynTree {
                 info.joint->setDamping(0, m_dynamic_params->damping);
                 info.joint->setStaticFriction(0, m_dynamic_params->staticFriction);
             }
-            
+
             if (!map->insert(std::unordered_map<std::string, JointElement::JointInfo>::value_type(m_jointName, info)).second) {
                 std::string errStr = "Duplicate joint " + m_jointName + " found.";
                 reportError("JointElement", "", errStr.c_str());
diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp
index 03ab7f17a35..30e9ddf6413 100644
--- a/src/model_io/codecs/src/URDFModelExport.cpp
+++ b/src/model_io/codecs/src/URDFModelExport.cpp
@@ -407,7 +407,6 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
 
     if (joint->getJointDynamicsType() == URDFJointDynamics && joint->getNrOfDOFs() == 1)
     {
-
         xmlNodePtr dynamics_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "dynamics", NULL);
         std::string bufStr;
         double damping = 0.0, static_friction = 0.0;
diff --git a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp
index 014b119620b..1484eca4ac9 100644
--- a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp
+++ b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp
@@ -221,6 +221,7 @@ void testFramesNotInTraversal() {
 
 int main()
 {
+    // IDYNTREE_TESTS_URDFS_NR
     for(unsigned int mdl = 0; mdl < IDYNTREE_TESTS_URDFS_NR; mdl++ )
     {
         if (std::string(IDYNTREE_TESTS_URDFS[mdl]) == "bigman.urdf")

From 98dd1dc8d93d610e16108421ba2fa58f997b9cad Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Thu, 31 Aug 2023 16:41:15 +0200
Subject: [PATCH 11/14] fix typo

---
 src/model_io/codecs/include/private/JointElement.h | 4 ++--
 src/model_io/codecs/src/JointElement.cpp           | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/model_io/codecs/include/private/JointElement.h b/src/model_io/codecs/include/private/JointElement.h
index 379043d9881..b1ee97fee0e 100644
--- a/src/model_io/codecs/include/private/JointElement.h
+++ b/src/model_io/codecs/include/private/JointElement.h
@@ -61,14 +61,14 @@ class iDynTree::JointElement : public iDynTree::XMLElement {
         double velocity;
     };
 
-    struct JointDyamicsParams {
+    struct JointDynamicsParams {
         JointDynamicsType jointDynamicsType;
         double damping;
         double staticFriction;
     };
 
     std::shared_ptr<Limits> m_limits;
-    std::shared_ptr<JointDyamicsParams> m_dynamic_params;
+    std::shared_ptr<JointDynamicsParams> m_dynamic_params;
     
 public:
     explicit JointElement(
diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp
index 952753ae437..015819914cc 100644
--- a/src/model_io/codecs/src/JointElement.cpp
+++ b/src/model_io/codecs/src/JointElement.cpp
@@ -133,7 +133,7 @@ namespace iDynTree {
             return std::shared_ptr<XMLElement>(element);
 
         } else if (name == "dynamics") {
-            m_dynamic_params = std::make_shared<JointDyamicsParams>();
+            m_dynamic_params = std::make_shared<JointDynamicsParams>();
             m_dynamic_params->jointDynamicsType = URDFJointDynamics;
             m_dynamic_params->damping = .0;
             m_dynamic_params->staticFriction = .0;

From 48dba2017a15b5def39a6698d363797ff6c2bd33 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Fri, 1 Sep 2023 09:29:13 +0200
Subject: [PATCH 12/14] Update CHANGELOG.md

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 70a97ad325e..1c2583d95b2 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 - Added the possibility of exporting additional XML elements that are added as child of the `<robot>` element in URDF ModelExporter (https://github.com/robotology/idyntree/pull/1088).
+- Added support for reading and wrting joint friction and damping values from URDF files (https://github.com/robotology/idyntree/pull/1094).
 
 ### Changed
 

From a6b720b4daeb78142901116c71cf6e1fedc36a6a Mon Sep 17 00:00:00 2001
From: Mattia Fussi <mattia.fussi@iit.it>
Date: Fri, 1 Sep 2023 09:36:06 +0200
Subject: [PATCH 13/14] remove leftover

---
 src/model_io/codecs/tests/ModelExporterUnitTest.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp
index 1484eca4ac9..014b119620b 100644
--- a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp
+++ b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp
@@ -221,7 +221,6 @@ void testFramesNotInTraversal() {
 
 int main()
 {
-    // IDYNTREE_TESTS_URDFS_NR
     for(unsigned int mdl = 0; mdl < IDYNTREE_TESTS_URDFS_NR; mdl++ )
     {
         if (std::string(IDYNTREE_TESTS_URDFS[mdl]) == "bigman.urdf")

From ece1c43f38da0289730bd8b7046dd49c1ac15f3a Mon Sep 17 00:00:00 2001
From: Mattia Fussi <38140169+mfussi66@users.noreply.github.com>
Date: Fri, 1 Sep 2023 09:44:08 +0200
Subject: [PATCH 14/14] Update src/model/src/RevoluteJoint.cpp

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>
---
 src/model/src/RevoluteJoint.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp
index f4af413a2d2..e80ec1ae72f 100644
--- a/src/model/src/RevoluteJoint.cpp
+++ b/src/model/src/RevoluteJoint.cpp
@@ -70,8 +70,8 @@ RevoluteJoint::RevoluteJoint(const RevoluteJoint& other):
                              rotation_axis_wrt_link1(other.rotation_axis_wrt_link1),
                              m_hasPosLimits(other.m_hasPosLimits),
                              m_minPos(other.m_minPos), m_maxPos(other.m_maxPos),
-                            m_joint_dynamics_type(other.m_joint_dynamics_type),
-                            m_damping(other.m_damping), m_static_friction(other.m_static_friction)
+                             m_joint_dynamics_type(other.m_joint_dynamics_type),
+                             m_damping(other.m_damping), m_static_friction(other.m_static_friction)
 {
     this->setPosCoordsOffset(other.getPosCoordsOffset());
     this->setDOFsOffset(other.getDOFsOffset());