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 `` 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 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_& 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) { diff --git a/src/model/include/iDynTree/Model/FixedJoint.h b/src/model/include/iDynTree/Model/FixedJoint.h index cf53abf1d44..5b9da0e297e 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 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); }; } diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h index d3ee05948d6..8a28e0e95f5 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. @@ -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; + + ///@} }; diff --git a/src/model/include/iDynTree/Model/PrismaticJoint.h b/src/model/include/iDynTree/Model/PrismaticJoint.h index 042ec9f700a..8230c113432 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 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); }; } diff --git a/src/model/include/iDynTree/Model/RevoluteJoint.h b/src/model/include/iDynTree/Model/RevoluteJoint.h index 86248795116..ad1fab38e6f 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 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); }; } diff --git a/src/model/src/FixedJoint.cpp b/src/model/src/FixedJoint.cpp index ea857982332..6d07a409532 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; } +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; +} + } diff --git a/src/model/src/PrismaticJoint.cpp b/src/model/src/PrismaticJoint.cpp index bffd12f4683..75cc135e25f 100644 --- a/src/model/src/PrismaticJoint.cpp +++ b/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; +} } diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp index 1a30bfb531d..e80ec1ae72f 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, @@ -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; +} } diff --git a/src/model_io/codecs/include/private/JointElement.h b/src/model_io/codecs/include/private/JointElement.h index f79a9fe2755..b1ee97fee0e 100644 --- a/src/model_io/codecs/include/private/JointElement.h +++ b/src/model_io/codecs/include/private/JointElement.h @@ -15,6 +15,8 @@ #include +#include + #include #include @@ -59,7 +61,14 @@ class iDynTree::JointElement : public iDynTree::XMLElement { double velocity; }; + struct JointDynamicsParams { + JointDynamicsType jointDynamicsType; + double damping; + double staticFriction; + }; + std::shared_ptr m_limits; + std::shared_ptr 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 0ce2470d49a..015819914cc 100644 --- a/src/model_io/codecs/src/JointElement.cpp +++ b/src/model_io/codecs/src/JointElement.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -132,6 +131,35 @@ namespace iDynTree { return true; }); return std::shared_ptr(element); + + } else if (name == "dynamics") { + m_dynamic_params = std::make_shared(); + 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>& 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(element); } return std::make_shared(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::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 8e63545a4bf..30e9ddf6413 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->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; }