idyntree 9.1.1.dev6__tar.gz → 9.1.1.dev7__tar.gz

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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
 

{idyntree-9.1.1.dev6/idyntree.egg-info → idyntree-9.1.1.dev7}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: idyntree
-Version: 9.1.1.dev6
+Version: 9.1.1.dev7
 Summary: Multibody Dynamics Library designed for Free Floating Robots.
 Home-page: https://github.com/robotology/idyntree
 Author: Diego Ferigo

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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) {

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7/idyntree.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: idyntree
-Version: 9.1.1.dev6
+Version: 9.1.1.dev7
 Summary: Multibody Dynamics Library designed for Free Floating Robots.
 Home-page: https://github.com/robotology/idyntree
 Author: Diego Ferigo

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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 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 setDamping(const size_t _index, double& damping);
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction);
     };
 }
 

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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.
@@ -363,6 +381,83 @@ namespace iDynTree
          */
         virtual bool setPosLimits(const size_t _index, double & min, double & max) = 0;
 
+        /**
+         * @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).
+         *
+         *  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 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 the specific joint dynamics type used for the joint.
+         */
+        virtual JointDynamicsType getJointDynamicsType() const = 0;
+         
+         /**
+         * 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 setJointDynamicsType(const JointDynamicsType enable) = 0;
+
+         /**
+         * 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.
+         * 
+         * 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) = 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 setStaticFriction(const size_t _index, double& staticFriction) = 0;
+
+        /**
+         * 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 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;
+
+
         ///@}
     };
 

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/src/model/include/iDynTree/Model/PrismaticJoint.h

@@ -41,6 +41,12 @@ namespace iDynTree
         double m_minPos;
         double m_maxPos;
 
+        // Dynamic parameters
+        void resetJointDynamics();
+        JointDynamicsType m_joint_dynamics_type;
+        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 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 setDamping(const size_t _index, double& damping);
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction);
     };
 }
 

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/src/model/include/iDynTree/Model/RevoluteJoint.h

@@ -41,6 +41,12 @@ namespace iDynTree
         double m_minPos;
         double m_maxPos;
 
+        // Dynamic parameters
+        void resetJointDynamics();
+        JointDynamicsType m_joint_dynamics_type;
+        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 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 setDamping(const size_t _index, double& damping);
+        virtual bool setStaticFriction(const size_t _index, double& staticFriction);
     };
 }
 

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/src/model/src/FixedJoint.cpp

@@ -267,4 +267,33 @@ bool FixedJoint::setPosLimits(const size_t /*_index*/, double & /*min*/, double
     return false;
 }
 
+JointDynamicsType FixedJoint::getJointDynamicsType() const
+{
+    return NoJointDynamics;
+}
+
+bool FixedJoint::setJointDynamicsType(const JointDynamicsType 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::setDamping(const size_t /*_index*/, double& /*damping*/)
+{
+    return false;
+}
+
+bool FixedJoint::setStaticFriction(const size_t /*_index*/, double& /*staticFriction*/)
+{
+    return false;
+}
+
 }

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/src/model/src/PrismaticJoint.cpp

@@ -34,6 +34,7 @@ PrismaticJoint::PrismaticJoint():
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->resetJointDynamics();
 }
 
 PrismaticJoint::PrismaticJoint(const LinkIndex _link1, const LinkIndex _link2,
@@ -47,6 +48,7 @@ PrismaticJoint::PrismaticJoint(const LinkIndex _link1, const LinkIndex _link2,
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->resetJointDynamics();
 }
 
 PrismaticJoint::PrismaticJoint(const PrismaticJoint& other):
@@ -54,7 +56,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_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());
@@ -383,6 +387,45 @@ bool PrismaticJoint::setPosLimits(const size_t /*_index*/, double & min, double
     return true;
 }
 
+void PrismaticJoint::resetJointDynamics()
+{
+    m_joint_dynamics_type = NoJointDynamics;
+    m_damping = 0.0;
+    m_static_friction = 0.0;
+}
+
+JointDynamicsType PrismaticJoint::getJointDynamicsType() const
+{
+    return m_joint_dynamics_type;
+}
+
+bool PrismaticJoint::setJointDynamicsType(const JointDynamicsType enable)
+{
+    m_joint_dynamics_type = 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::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;
+}
 
 }

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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,
@@ -47,6 +48,7 @@ RevoluteJoint::RevoluteJoint(const LinkIndex _link1, const LinkIndex _link2,
     this->resetAxisBuffers();
     this->resetBuffers(0);
     this->disablePosLimits();
+    this->resetJointDynamics();
 }
 
 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->resetJointDynamics();
 }
 
 RevoluteJoint::RevoluteJoint(const RevoluteJoint& other):
@@ -66,7 +69,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_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());
@@ -397,6 +402,46 @@ bool RevoluteJoint::setPosLimits(const size_t /*_index*/, double & min, double &
     return true;
 }
 
+void RevoluteJoint::resetJointDynamics()
+{
+    m_joint_dynamics_type = NoJointDynamics;
+    m_damping = 0.0;
+    m_static_friction = 0.0;
+}
+
+JointDynamicsType RevoluteJoint::getJointDynamicsType() const
+{
+    return m_joint_dynamics_type;
+}
+
+bool RevoluteJoint::setJointDynamicsType(const JointDynamicsType enable)
+{
+    m_joint_dynamics_type = 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::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;
+}
 
 }

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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,7 +61,14 @@ private:
         double velocity;
     };
 
+    struct JointDynamicsParams {
+        JointDynamicsType jointDynamicsType;
+        double damping;
+        double staticFriction;
+    };
+
     std::shared_ptr<Limits> m_limits;
+    std::shared_ptr<JointDynamicsParams> m_dynamic_params;
     
 public:
     explicit JointElement(

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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>
@@ -132,6 +131,35 @@ namespace iDynTree {
                 return true;
             });
             return std::shared_ptr<XMLElement>(element);
+
+        } else if (name == "dynamics") {
+            m_dynamic_params = std::make_shared<JointDynamicsParams>();
+            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;
+                        }
+                    }
+                    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);
     }
@@ -175,7 +203,13 @@ 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);
+            }
+
             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());

{idyntree-9.1.1.dev6 → idyntree-9.1.1.dev7}/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->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;
+        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);
+        xmlNewProp(dynamics_xml, BAD_CAST "friction", BAD_CAST bufStr.c_str());
+    }
 
     return ok;
 }