diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
index dd5ad0e21..e6f07def2 100644
--- a/src/parser_urdf.cc
+++ b/src/parser_urdf.cc
@@ -412,6 +412,42 @@ void ReduceFixedJoints(TiXmlElement *_root, urdf::LinkSharedPtr _link)
sdfdbg << "Fixed Joint Reduction: extension lumping from ["
<< _link->name << "] to [" << _link->getParent()->name << "]\n";
+ // Add //model/frame tag to memorialize reduced joint
+ std::stringstream ssj;
+ ssj << "\n";
+ ssj << " "
+ << CopyPose(_link->parent_joint->parent_to_joint_origin_transform)
+ << "\n";
+ ssj << "\n";
+
+ // Add //model/frame tag to memorialize reduced link
+ std::stringstream ssl;
+ ssl << "\n";
+
+ // Serialize sdf::Frame objects to xml and add to SDFExtension
+ SDFExtensionPtr sdfExt = std::make_shared();
+ auto stringToExtension = [&sdfExt](const std::string &_frame)
+ {
+ TiXmlDocument xmlNewDoc;
+ xmlNewDoc.Parse(_frame.c_str());
+ if (xmlNewDoc.Error())
+ {
+ sdferr << "Error while parsing serialized frames: "
+ << xmlNewDoc.ErrorDesc() << '\n';
+ }
+
+ TiXmlElementPtr blob =
+ std::make_shared(*xmlNewDoc.FirstChildElement());
+ sdfExt->blobs.push_back(blob);
+ };
+ stringToExtension(ssj.str());
+ stringToExtension(ssl.str());
+
+ // Add //frame tags to model extension vector
+ g_extensions[""].push_back(sdfExt);
+
// lump sdf extensions to parent, (give them new reference _link names)
ReduceSDFExtensionToParent(_link);
diff --git a/test/integration/fixed_joint_example.urdf b/test/integration/fixed_joint_example.urdf
new file mode 100644
index 000000000..9f09ab285
--- /dev/null
+++ b/test/integration/fixed_joint_example.urdf
@@ -0,0 +1,63 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/test/integration/fixed_joint_simple.urdf b/test/integration/fixed_joint_simple.urdf
new file mode 100644
index 000000000..3443f962f
--- /dev/null
+++ b/test/integration/fixed_joint_simple.urdf
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc
index e9d0a8d5b..b75ea6f2c 100644
--- a/test/integration/urdf_gazebo_extensions.cc
+++ b/test/integration/urdf_gazebo_extensions.cc
@@ -29,11 +29,17 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
const std::string urdfTestFile =
sdf::testing::TestFile("integration", "urdf_gazebo_extensions.urdf");
- sdf::SDFPtr robot(new sdf::SDF());
- sdf::init(robot);
- ASSERT_TRUE(sdf::readFile(urdfTestFile, robot));
+ sdf::Root root;
+ auto errors = root.Load(urdfTestFile);
+ EXPECT_TRUE(errors.empty());
+ for (auto e : errors)
+ {
+ std::cerr << e << std::endl;
+ }
- sdf::ElementPtr model = robot->Root()->GetElement("model");
+ auto modelDom = root.ModelByIndex(0);
+ ASSERT_NE(nullptr, modelDom);
+ sdf::ElementPtr model = modelDom->Element();
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
joint = joint->GetNextElement("joint"))
{
@@ -306,4 +312,105 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
checkElementPoses("light");
checkElementPoses("projector");
checkElementPoses("sensor");
+
+ // Check that //model/frame elements are added for reduced joints
+ EXPECT_EQ(14u, modelDom->FrameCount());
+
+ EXPECT_TRUE(modelDom->FrameNameExists("issue378_link_joint"));
+ EXPECT_TRUE(modelDom->FrameNameExists("jCamera"));
+ EXPECT_TRUE(modelDom->FrameNameExists("jointSensorNoPose"));
+ EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPose"));
+ EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseRelative"));
+ EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel"));
+ EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel2"));
+
+ EXPECT_TRUE(modelDom->FrameNameExists("issue378_link"));
+ EXPECT_TRUE(modelDom->FrameNameExists("Camera"));
+ EXPECT_TRUE(modelDom->FrameNameExists("linkSensorNoPose"));
+ EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPose"));
+ EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseRelative"));
+ EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel"));
+ EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel2"));
+}
+
+/////////////////////////////////////////////////
+TEST(SDFParser, FixedJointExample)
+{
+ const std::string urdfTestFile =
+ sdf::testing::TestFile("integration", "fixed_joint_example.urdf");
+
+ sdf::Root root;
+ auto errors = root.Load(urdfTestFile);
+ EXPECT_TRUE(errors.empty());
+ for (auto e : errors)
+ {
+ std::cerr << e << std::endl;
+ }
+
+ auto model = root.ModelByIndex(0);
+ ASSERT_NE(nullptr, model);
+ EXPECT_EQ("fixed_joint_example", model->Name());
+
+ EXPECT_EQ(2u, model->LinkCount());
+ EXPECT_TRUE(model->LinkNameExists("base"));
+ EXPECT_TRUE(model->LinkNameExists("rotary_link"));
+
+ // Expect MassMatrix3 values to match for links
+ auto link1 = model->LinkByName("base");
+ auto link2 = model->LinkByName("rotary_link");
+ ASSERT_NE(nullptr, link1);
+ ASSERT_NE(nullptr, link2);
+ auto massMatrix1 = link1->Inertial().MassMatrix();
+ auto massMatrix2 = link2->Inertial().MassMatrix();
+ EXPECT_DOUBLE_EQ(massMatrix1.Mass(), massMatrix2.Mass());
+ EXPECT_EQ(massMatrix1.Moi(), massMatrix2.Moi());
+
+ EXPECT_EQ(1u, model->JointCount());
+ EXPECT_TRUE(model->JointNameExists("rotary_joint"));
+
+ EXPECT_EQ(2u, model->FrameCount());
+ ASSERT_TRUE(model->FrameNameExists("intermediate_joint"));
+ ASSERT_TRUE(model->FrameNameExists("intermediate_link"));
+
+ const std::string j = "intermediate_joint";
+ const std::string l = "intermediate_link";
+ std::string body;
+ EXPECT_TRUE(model->FrameByName(j)->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("base", body);
+ EXPECT_TRUE(model->FrameByName(l)->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("base", body);
+}
+
+/////////////////////////////////////////////////
+TEST(SDFParser, FixedJointSimple)
+{
+ const std::string urdfTestFile =
+ sdf::testing::TestFile("integration", "fixed_joint_simple.urdf");
+
+ sdf::Root root;
+ auto errors = root.Load(urdfTestFile);
+ EXPECT_TRUE(errors.empty());
+ for (auto e : errors)
+ {
+ std::cerr << e << std::endl;
+ }
+
+ auto model = root.ModelByIndex(0);
+ ASSERT_NE(nullptr, model);
+ EXPECT_EQ("fixed_joint_simple", model->Name());
+
+ EXPECT_EQ(1u, model->LinkCount());
+ EXPECT_TRUE(model->LinkNameExists("base"));
+
+ auto link = model->LinkByName("base");
+ ASSERT_NE(nullptr, link);
+ auto massMatrix = link->Inertial().MassMatrix();
+ EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass());
+ EXPECT_EQ(2.0 * ignition::math::Matrix3d::Identity, massMatrix.Moi());
+
+ EXPECT_EQ(0u, model->JointCount());
+
+ EXPECT_EQ(2u, model->FrameCount());
+ ASSERT_TRUE(model->FrameNameExists("fixed_joint"));
+ ASSERT_TRUE(model->FrameNameExists("child_link"));
}