From be3a6191acc6d31f52275eccb55c359eaa542ec5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 26 Sep 2023 11:44:31 -0700 Subject: [PATCH 1/3] URDF parser: use SDFormat 1.11, parse joint mimic Signed-off-by: Steve Peters --- src/parser_urdf.cc | 14 +++++++++++++- src/parser_urdf_TEST.cc | 6 +++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index e26e8f6bd..341a5ab45 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) @@ -3410,7 +3422,7 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr, { // URDF is compatible with version 1.7. 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 From 309e611b93fa8bef74bbae31a5ceae696bb245ba Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 2 Oct 2023 20:12:01 -0700 Subject: [PATCH 2/3] Add example joint_mimic_rack_pinion.urdf and test Signed-off-by: Steve Peters --- test/integration/joint_axis_dom.cc | 26 ++++++ test/sdf/joint_mimic_rack_pinion.urdf | 125 ++++++++++++++++++++++++++ 2 files changed, 151 insertions(+) create mode 100644 test/sdf/joint_mimic_rack_pinion.urdf 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 16ec0c75242723a55c81fd4c0f63f7c2fe3e191f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 3 Oct 2023 09:16:37 -0700 Subject: [PATCH 3/3] parser_urdf.cc: fix comment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Steve Peters --- src/parser_urdf.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 341a5ab45..d8d2b2352 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3420,7 +3420,7 @@ 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.11"); // add robot to sdf