Add export to urdf of joint dynamics: damping and static friction coefficients #1094

mfussi66 · 2023-08-30T14:02:51Z

This PR aims to expand the Joints objects (Fixed, Revolute, Prismatic, IJoint) to support dynamic parameters present in URDF models.
These parameters are damping coefficient (N*s/m for prismatic and N*m*s/rad for revolute) and static friction coefficient (N for prismatic and N*m for revolute) .
The parameters can be imported from a urdf file or exported into one.

cc @traversaro @Nicogene

traversaro · 2023-08-30T14:44:33Z

Thanks a lot for this work! I want to comment a bit in depth in the PR, but before of that, it seems that there are a few memory-related error catched by the valgrind tests:

2023-08-30T14:20:08.1447826Z Model loaded from /__w/idyntree/idyntree/src/tests/data/iCubGenova02.urdf
2023-08-30T14:20:08.1448051Z ==19584== Conditional jump or move depends on uninitialised value(s)
2023-08-30T14:20:08.1448791Z ==19584==    at 0x487273D: iDynTree::exportJoint(iDynTree::IJoint const*, iDynTree::Link const*, iDynTree::Link const*, iDynTree::Model const&, _xmlNode*) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1449626Z ==19584==    by 0x48744DB: iDynTree::URDFStringFromModel(iDynTree::Model const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, iDynTree::ModelExporterOptions) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1450534Z ==19584==    by 0x486E0A0: iDynTree::ModelExporter::exportModelToString(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1450972Z ==19584==    by 0x10ED75: checkImportExportURDF(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1451213Z ==19584==    by 0x10D586: main (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1451308Z ==19584== 
2023-08-30T14:20:08.1451521Z ==19584== Conditional jump or move depends on uninitialised value(s)
2023-08-30T14:20:08.1452209Z ==19584==    at 0x4872765: iDynTree::exportJoint(iDynTree::IJoint const*, iDynTree::Link const*, iDynTree::Link const*, iDynTree::Model const&, _xmlNode*) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1453029Z ==19584==    by 0x48744DB: iDynTree::URDFStringFromModel(iDynTree::Model const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, iDynTree::ModelExporterOptions) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1454068Z ==19584==    by 0x486E0A0: iDynTree::ModelExporter::exportModelToString(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1454490Z ==19584==    by 0x10ED75: checkImportExportURDF(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1454731Z ==19584==    by 0x10D586: main (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1454824Z ==19584== 
2023-08-30T14:20:08.1455040Z ==19584== Conditional jump or move depends on uninitialised value(s)
2023-08-30T14:20:08.1455475Z ==19584==    at 0x488DFCD: idyntree_private_fpconv_dtoa (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1456267Z ==19584==    by 0x487277A: iDynTree::exportJoint(iDynTree::IJoint const*, iDynTree::Link const*, iDynTree::Link const*, iDynTree::Model const&, _xmlNode*) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1457096Z ==19584==    by 0x48744DB: iDynTree::URDFStringFromModel(iDynTree::Model const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, iDynTree::ModelExporterOptions) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1458097Z ==19584==    by 0x486E0A0: iDynTree::ModelExporter::exportModelToString(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1458639Z ==19584==    by 0x10ED75: checkImportExportURDF(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1458886Z ==19584==    by 0x10D586: main (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1458988Z ==19584== 
2023-08-30T14:20:08.1459204Z ==19584== Conditional jump or move depends on uninitialised value(s)
2023-08-30T14:20:08.1459647Z ==19584==    at 0x488DFEE: idyntree_private_fpconv_dtoa (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1460339Z ==19584==    by 0x487277A: iDynTree::exportJoint(iDynTree::IJoint const*, iDynTree::Link const*, iDynTree::Link const*, iDynTree::Model const&, _xmlNode*) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1461178Z ==19584==    by 0x48744DB: iDynTree::URDFStringFromModel(iDynTree::Model const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, iDynTree::ModelExporterOptions) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1462086Z ==19584==    by 0x486E0A0: iDynTree::ModelExporter::exportModelToString(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/lib/libidyntree-modelio.so)
2023-08-30T14:20:08.1462512Z ==19584==    by 0x10ED75: checkImportExportURDF(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)
2023-08-30T14:20:08.1462755Z ==19584==    by 0x10D586: main (in /__w/idyntree/idyntree/build/bin/ModelExporterUnitTest)

Can you check them? To get more informative errors, you can compile locally in Debug model and so you will get the line numbers related to this.

traversaro · 2023-08-30T15:01:41Z

src/model_io/codecs/include/private/JointElement.h

@@ -59,7 +59,13 @@ class iDynTree::JointElement : public iDynTree::XMLElement {
        double velocity;
    };

+    struct DyamicParams {


Suggested change

struct DyamicParams {

struct JointDynamicParams {

Two changes:

Fix typo

Be more precise on which type of "Dynamics" is, as iDynTree is a "dynamics" library "dynamics" is quite an overloaded term.

Fixed the name!

traversaro · 2023-08-30T15:46:50Z

@mfussi66 sorry, I broke your branch with the commit mfussi66@8dfc3f9, but for some reason I couldn't easy create a PR out of it. However, it implements (in the interface) what we discuss, if you could modify the rest of the PR to be aligned it would be great, thanks!

traversaro · 2023-08-30T16:09:19Z

This is still not ready for review, but probably @francesco-romano may be interested in this.

traversaro · 2023-08-31T13:19:55Z

src/model_io/codecs/src/JointElement.cpp

+                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());


The dynamics element is optional in the URDF spec (see http://wiki.ros.org/urdf/XML/joint#Elements), so I guess we should not print a warning if it is not present in the URDF?

Good point, I removed the warning.

traversaro · 2023-08-31T13:22:47Z

src/model_io/codecs/src/URDFModelExport.cpp

@@ -405,7 +405,19 @@ 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() != NoJointDynamics && joint->getNrOfDOFs() == 1)


Suggested change

if (joint->getJointDynamicsType() != NoJointDynamics && joint->getNrOfDOFs() == 1)

if (joint->getJointDynamicsType() == URDFJointDynamics && joint->getNrOfDOFs() == 1)

(Just to be safe in case a new joint dynamics type that does not use damping or static friction is added).

traversaro · 2023-08-31T14:27:58Z

src/model_io/codecs/include/private/JointElement.h

@@ -59,14 +61,14 @@ class iDynTree::JointElement : public iDynTree::XMLElement {
        double velocity;
    };

-    struct DyamicParams {
-        uint8_t jointDynamicsType;
+    struct JointDyamicsParams {


Suggested change

struct JointDyamicsParams {

struct JointDynamicsParams {

mfussi66 · 2023-09-01T07:22:21Z

Seems like there are errors related to fortran libraries:

Matlab tests:

{�Error using iDynTree.Position (line 10)
Invalid MEX-file '/home/runner/work/idyntree/idyntree/build/lib/iDynTreeMEX.mexa64': /usr/local/MATLAB/R2020b/bin/glnxa64/../../sys/os/glnxa64/libgfortran.so.5: version
`GFORTRAN_10' not found (required by /usr/share/miniconda3/envs/test/lib/./libspral.so)

Debian tests:

2023-08-31T15:04:08.5475268Z gmake[2]: *** No rule to make target '/usr/lib/gcc/x86_64-linux-gnu/12/libgfortran.so', needed by 'bin/iKinConsistencyTest'.  Stop.

Are they an issue for this PR?

traversaro · 2023-09-01T07:27:49Z

Seems like there are errors related to fortran libraries:

Matlab tests:

{�Error using iDynTree.Position (line 10)
Invalid MEX-file '/home/runner/work/idyntree/idyntree/build/lib/iDynTreeMEX.mexa64': /usr/local/MATLAB/R2020b/bin/glnxa64/../../sys/os/glnxa64/libgfortran.so.5: version
`GFORTRAN_10' not found (required by /usr/share/miniconda3/envs/test/lib/./libspral.so)

Debian tests:

2023-08-31T15:04:08.5475268Z gmake[2]: *** No rule to make target '/usr/lib/gcc/x86_64-linux-gnu/12/libgfortran.so', needed by 'bin/iKinConsistencyTest'.  Stop.

Are they an issue for this PR?

No, please ignore, thanks.

traversaro · 2023-09-01T07:28:26Z

src/model_io/codecs/tests/ModelExporterUnitTest.cpp

@@ -221,6 +221,7 @@ void testFramesNotInTraversal() {

 int main()
 {
+    // IDYNTREE_TESTS_URDFS_NR


Suggested change

// IDYNTREE_TESTS_URDFS_NR

// IDYNTREE_TESTS_URDFS_NR

leftover?

src/model/src/RevoluteJoint.cpp

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>

traversaro · 2023-09-01T07:49:54Z

Thanks @mfussi66 !

mfussi66 added 3 commits August 30, 2023 15:03

Add export to urdf of joint dynamics:damping and friction

a4f6cd7

Add reading of dynamics attributes of joint

416dfd2

Add comments to dyn params

6816a3f

Nicogene mentioned this pull request Aug 30, 2023

CREO2URDF – MVP4 last tweaks icub-tech-iit/creo2urdf#48

Closed

traversaro reviewed Aug 30, 2023

View reviewed changes

Generalize joint dynamics interface

8dfc3f9

initialize dyn params in constructors

1df22d4

mfussi66 added 2 commits August 31, 2023 10:26

use jointdynamicstype

5a92e8a

additional fix in modelio to reduce memory leaks

e947ae1

traversaro reviewed Aug 31, 2023

View reviewed changes

mfussi66 added 3 commits August 31, 2023 15:38

remove warning if dynamics is missing

0afa763

invert check on dynamics

8425b4e

Add additional reset of jdyn to avoid uninitialized variables

c57b386

traversaro reviewed Aug 31, 2023

View reviewed changes

fix typo

98dd1dc

traversaro reviewed Sep 1, 2023

View reviewed changes

traversaro and others added 2 commits September 1, 2023 09:29

Update CHANGELOG.md

48dba20

remove leftover

a6b720b

traversaro reviewed Sep 1, 2023

View reviewed changes

src/model/src/RevoluteJoint.cpp Outdated Show resolved Hide resolved

Update src/model/src/RevoluteJoint.cpp

ece1c43

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>

traversaro approved these changes Sep 1, 2023

View reviewed changes

traversaro merged commit 3509d48 into robotology:devel Sep 1, 2023

traversaro mentioned this pull request Jan 12, 2024

URDFModelExport: Remove outdated comment #1144

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add export to urdf of joint dynamics: damping and static friction coefficients #1094

Add export to urdf of joint dynamics: damping and static friction coefficients #1094

mfussi66 commented Aug 30, 2023 •

edited

Loading

traversaro commented Aug 30, 2023

traversaro Aug 30, 2023

mfussi66 Aug 31, 2023

traversaro commented Aug 30, 2023

traversaro commented Aug 30, 2023

traversaro Aug 31, 2023

mfussi66 Aug 31, 2023

traversaro Aug 31, 2023

mfussi66 Aug 31, 2023

traversaro Aug 31, 2023

mfussi66 commented Sep 1, 2023 •

edited

Loading

traversaro commented Sep 1, 2023

traversaro Sep 1, 2023

traversaro commented Sep 1, 2023

	if (joint->getJointDynamicsType() != NoJointDynamics && joint->getNrOfDOFs() == 1)
	if (joint->getJointDynamicsType() == URDFJointDynamics && joint->getNrOfDOFs() == 1)

Add export to urdf of joint dynamics: damping and static friction coefficients #1094

Add export to urdf of joint dynamics: damping and static friction coefficients #1094

Conversation

mfussi66 commented Aug 30, 2023 • edited Loading

traversaro commented Aug 30, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro commented Aug 30, 2023

traversaro commented Aug 30, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mfussi66 commented Sep 1, 2023 • edited Loading

traversaro commented Sep 1, 2023

Choose a reason for hiding this comment

traversaro commented Sep 1, 2023

mfussi66 commented Aug 30, 2023 •

edited

Loading

mfussi66 commented Sep 1, 2023 •

edited

Loading