From 9ca30ccf720bca8d771f671a332da66809ef81eb Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Feb 2021 19:02:51 -0800 Subject: [PATCH 1/5] parse rpyOffset as radians Signed-off-by: Ian Chen --- 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 5380da09f..39774457f 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3467,7 +3467,7 @@ void ReduceSDFExtensionPluginFrameReplace( TiXmlNode* rpyKey = (*_blobIt)->FirstChild("rpyOffset"); if (rpyKey) { - urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0); + urdf::Vector3 rpy = ParseVector3(rpyKey); _reductionTransform.Rot() = ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); // remove xyzOffset and rpyOffset From 72381f17bde87a6c2e6ae1a94245287d1ab61823 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Feb 2021 20:44:26 -0800 Subject: [PATCH 2/5] update tf for xyz and rpy offsets Signed-off-by: Ian Chen --- src/parser_urdf.cc | 2 +- test/integration/fixed_joint_reduction.cc | 26 +++++ ...oint_reduction_plugin_frame_extension.urdf | 103 ++++++++++++++++++ 3 files changed, 130 insertions(+), 1 deletion(-) create mode 100644 test/integration/fixed_joint_reduction_plugin_frame_extension.urdf diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 39774457f..c0adb6def 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3475,7 +3475,7 @@ void ReduceSDFExtensionPluginFrameReplace( } // pass through the parent transform from fixed joint reduction - _reductionTransform = inverseTransformToParentFrame(_reductionTransform, + _reductionTransform = TransformToParentFrame(_reductionTransform, _link->parent_joint->parent_to_joint_origin_transform); // create new offset xml blocks diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 6e2d5b628..287258ace 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -48,6 +48,9 @@ const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT = const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", "fixed_joint_reduction_collision_visual_empty_root.sdf"); +const std::string SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "fixed_joint_reduction_plugin_frame_extension.urdf"); const double gc_tolerance = 1e-6; @@ -736,3 +739,26 @@ TEST(SDFParser, FixedJointReductionSimple) EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance); } } + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with plugin that +// contains bodyName, xyzOffset and rpyOffset. +// Test to make sure the offsets have the correct transfrom and frame of +// reference +TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION, robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + sdf::ElementPtr plugin = model->GetElement("plugin"); + + auto xyzOffset = plugin->Get("xyzOffset"); + auto rpyOffset = plugin->Get("rpyOffset"); + auto bodyName = plugin->Get("bodyName"); + EXPECT_EQ("base_link", bodyName); + EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset); +} + diff --git a/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf new file mode 100644 index 000000000..4c2293670 --- /dev/null +++ b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf @@ -0,0 +1,103 @@ + + + + + /test/plugin/service + /test/plugin/topic + link2 + 100 + 0 0 0 + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From de3e26826f4d6466ed33905045aaaf911035a510 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 25 Feb 2021 17:26:16 -0800 Subject: [PATCH 3/5] remove inverse transform function in urdf parser Signed-off-by: Ian Chen --- src/parser_urdf.cc | 29 ----------------------------- 1 file changed, 29 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index c0adb6def..ad857e54c 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -219,10 +219,6 @@ std::string Values2str(unsigned int _count, const double *_values); void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry); -ignition::math::Pose3d inverseTransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, - urdf::Pose _parentToLinkTransform); - /// reduced fixed joints: transform to parent frame urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame, urdf::Pose _parentToLinkTransform); @@ -2420,31 +2416,6 @@ ignition::math::Pose3d TransformToParentFrame( return transformInParentLinkFrame; } -///////////////////////////////////////////////// -/// reduced fixed joints: transform to parent frame -ignition::math::Pose3d inverseTransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, - urdf::Pose _parentToLinkTransform) -{ - ignition::math::Pose3d transformInParentLinkFrame; - // rotate link pose to parentLink frame - urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse(); - ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z); - transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos(); - urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse(); - ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z); - transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot(); - // translate link to parentLink frame - transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X() - - _parentToLinkTransform.position.x; - transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y() - - _parentToLinkTransform.position.y; - transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z() - - _parentToLinkTransform.position.z; - - return transformInParentLinkFrame; -} - //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link) { From 536b8472a0edb363ee02280ec875db61bb680c91 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 15 Jul 2021 23:05:38 -0700 Subject: [PATCH 4/5] inject corrected_offets tag Signed-off-by: Ian Chen --- src/parser_urdf.cc | 10 ++++++++++ test/integration/fixed_joint_reduction.cc | 3 +++ 2 files changed, 13 insertions(+) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index ad857e54c..27728078e 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3444,6 +3444,12 @@ void ReduceSDFExtensionPluginFrameReplace( // remove xyzOffset and rpyOffset (*_blobIt)->RemoveChild(rpyKey); } + TiXmlNode* correctedOffsetKey = + (*_blobIt)->FirstChild("ignition::corrected_offsets"); + if (!correctedOffsetKey) + { + (*_blobIt)->RemoveChild(correctedOffsetKey); + } // pass through the parent transform from fixed joint reduction _reductionTransform = TransformToParentFrame(_reductionTransform, @@ -3452,6 +3458,7 @@ void ReduceSDFExtensionPluginFrameReplace( // create new offset xml blocks xyzKey = new TiXmlElement("xyzOffset"); rpyKey = new TiXmlElement("rpyOffset"); + correctedOffsetKey = new TiXmlElement("ignition::corrected_offsets"); // create new offset xml blocks urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), @@ -3472,12 +3479,15 @@ void ReduceSDFExtensionPluginFrameReplace( TiXmlText* xyzTxt = new TiXmlText(xyzStream.str()); TiXmlText* rpyTxt = new TiXmlText(rpyStream.str()); + TiXmlText* correctedOffsetTxt = new TiXmlText("1"); xyzKey->LinkEndChild(xyzTxt); rpyKey->LinkEndChild(rpyTxt); + correctedOffsetKey->LinkEndChild(correctedOffsetTxt); (*_blobIt)->LinkEndChild(xyzKey); (*_blobIt)->LinkEndChild(rpyKey); + (*_blobIt)->LinkEndChild(correctedOffsetKey); } } } diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 287258ace..f15080d3c 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -760,5 +760,8 @@ TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) EXPECT_EQ("base_link", bodyName); EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset); EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset); + + bool correctedOffset = plugin->Get("ignition::corrected_offsets"); + EXPECT_TRUE(correctedOffset); } From 2eba874b1d680f7f2d6a980530458b084798a445 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 23 Jul 2021 13:58:08 -0700 Subject: [PATCH 5/5] Fix tag removal logic Signed-off-by: Steven 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 27728078e..a83ee21c1 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3446,7 +3446,7 @@ void ReduceSDFExtensionPluginFrameReplace( } TiXmlNode* correctedOffsetKey = (*_blobIt)->FirstChild("ignition::corrected_offsets"); - if (!correctedOffsetKey) + if (correctedOffsetKey) { (*_blobIt)->RemoveChild(correctedOffsetKey); }