From c02aca7fc689592ba10e15baa1f6cea88e405186 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 30 Aug 2021 13:59:46 -0700 Subject: [PATCH] Fix xyz and rpy offsets in fixed joint reduction (#500) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * parse rpyOffset as radians Signed-off-by: Ian Chen * update tf for xyz and rpy offsets Signed-off-by: Ian Chen * remove inverse transform function in urdf parser Signed-off-by: Ian Chen * inject corrected_offets tag Signed-off-by: Ian Chen * Fix tag removal logic Signed-off-by: Steven Peters Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Steve Peters --- src/parser_urdf.cc | 41 ++----- test/integration/fixed_joint_reduction.cc | 29 +++++ ...oint_reduction_plugin_frame_extension.urdf | 103 ++++++++++++++++++ 3 files changed, 143 insertions(+), 30 deletions(-) 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..a83ee21c1 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) { @@ -3473,14 +3444,21 @@ 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 = inverseTransformToParentFrame(_reductionTransform, + _reductionTransform = TransformToParentFrame(_reductionTransform, _link->parent_joint->parent_to_joint_origin_transform); // 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(), @@ -3501,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 6e2d5b628..f15080d3c 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,29 @@ 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); + + bool correctedOffset = plugin->Get("ignition::corrected_offsets"); + EXPECT_TRUE(correctedOffset); +} + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +