From 013bc337b4ce54f5a6d9e5d4da008d2cf47fb99f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 3 Oct 2023 16:46:39 -0700 Subject: [PATCH] URDF parser: use SDFormat 1.11, parse joint mimic (#1333) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This updates the URDF parser to generate SDFormat 1.11 files and adds support for parsing the URDF //joint/mimic tags into SDFormat //joint/axis/mimic tags. An example joint_mimic_rack_pinion.urdf is added with a test. Signed-off-by: Steve Peters Co-authored-by: Alejandro Hernández Cordero --- src/parser_urdf.cc | 16 +++- src/parser_urdf_TEST.cc | 6 +- test/integration/joint_axis_dom.cc | 26 ++++++ test/sdf/joint_mimic_rack_pinion.urdf | 125 ++++++++++++++++++++++++++ 4 files changed, 168 insertions(+), 5 deletions(-) create mode 100644 test/sdf/joint_mimic_rack_pinion.urdf diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index e26e8f6bd..d8d2b2352 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3148,6 +3148,18 @@ void CreateJoint(tinyxml2::XMLElement *_root, AddKeyValue(jointAxisLimit, "velocity", Values2str(1, &_link->parent_joint->limits->velocity)); } + + if (_link->parent_joint->mimic) + { + tinyxml2::XMLElement *jointAxisMimic = doc->NewElement("mimic"); + jointAxisMimic->SetAttribute( + "joint", _link->parent_joint->mimic->joint_name.c_str()); + AddKeyValue(jointAxisMimic, "multiplier", + Values2str(1, &_link->parent_joint->mimic->multiplier)); + AddKeyValue(jointAxisMimic, "offset", + Values2str(1, &_link->parent_joint->mimic->offset)); + jointAxis->LinkEndChild(jointAxisMimic); + } } if (jtype == "fixed" && !fixedJointConvertedToRevoluteJoint) @@ -3408,9 +3420,9 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr, try { - // URDF is compatible with version 1.7. The automatic conversion script + // URDF is compatible with version 1.11. The automatic conversion script // will up-convert URDF to SDF. - sdf->SetAttribute("version", "1.7"); + sdf->SetAttribute("version", "1.11"); // add robot to sdf sdf->LinkEndChild(robot); } diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 0eafd11d2..96b940d99 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -100,8 +100,8 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel) // SDF -> SDF std::ostringstream stream; - // parser_urdf.cc exports version "1.7" - stream << "" + // parser_urdf.cc exports version "1.11" + stream << "" << " " << ""; tinyxml2::XMLDocument sdf_doc; @@ -890,7 +890,7 @@ TEST(URDFParser, CheckJointTransform) << " " << ""; - std::string expectedSdf = R"( + std::string expectedSdf = R"( 0 0 0 0 0 0 diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index 4aea8879b..c6ae94635 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -559,3 +559,29 @@ TEST(DOMJointAxis, ParseMimicInvalidLeaderAxis) EXPECT_EQ(errors[2].Message(), errorMsg2) << errors[2]; EXPECT_EQ(errors[3].Message(), errorMsg3) << errors[3]; } + +///////////////////////////////////////////////// +TEST(DOMJointAxis, ParseMimicURDF) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_mimic_rack_pinion.urdf"); + + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto model = root.Model(); + ASSERT_NE(nullptr, model); + auto followerJoint = model->JointByName("rack_joint"); + ASSERT_NE(nullptr, followerJoint); + auto followerJointAxis = followerJoint->Axis(); + ASSERT_NE(nullptr, followerJointAxis); + auto mimicJoint = followerJointAxis->Mimic(); + ASSERT_NE(std::nullopt, mimicJoint); + + EXPECT_EQ(mimicJoint->Joint(), "upper_joint"); + EXPECT_EQ(mimicJoint->Axis(), "axis"); + EXPECT_DOUBLE_EQ(mimicJoint->Multiplier(), 0.105); + EXPECT_DOUBLE_EQ(mimicJoint->Offset(), 0); + EXPECT_DOUBLE_EQ(mimicJoint->Reference(), 0); +} diff --git a/test/sdf/joint_mimic_rack_pinion.urdf b/test/sdf/joint_mimic_rack_pinion.urdf new file mode 100644 index 000000000..6adf32034 --- /dev/null +++ b/test/sdf/joint_mimic_rack_pinion.urdf @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +