Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

parser_urdf: add //frame for reduced links/joints #1148

Merged
merged 4 commits into from
Sep 29, 2022
Merged

parser_urdf: add //frame for reduced links/joints #1148

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
03463b8
parser_urdf: add //frame for reduced links/joints
scpeters Sep 14, 2022
fe3b393
Add another test with simpler example URDF
scpeters Sep 10, 2022
8b3c2ad
Add test with even simpler example
scpeters Sep 10, 2022
2e9f08e
Respond to review comments
scpeters Sep 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 38 additions & 0 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -412,6 +412,44 @@ 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 << "<frame name='" << _link->parent_joint->name << "'"
<< " attached_to='" << _link->getParent()->name << "'>\n";
ssj << " <pose>"
<< CopyPose(_link->parent_joint->parent_to_joint_origin_transform)
<< "</pose>\n";
ssj << "</frame>\n";

// Add //model/frame tag to memorialize reduced link
std::stringstream ssl;
ssl << "<frame name='" << _link->name + "'"
<< " attached_to='" << _link->parent_joint->name << "'/>\n";

// Serialize sdf::Frame objects to xml and add to SDFExtension
SDFExtensionPtr sdfExt = std::make_shared<SDFExtension>();
auto stringToExtension = [&sdfExt](const std::string &_frame)
{
TiXmlDocument xmlNewDoc;
xmlNewDoc.Parse(_frame.c_str());
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is it possible for this to fail? i.e, do we need to check if xmlNewDoc.FirstChildElement() is nullptr?

Copy link
Member Author

@scpeters scpeters Sep 28, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

there's lots of places where we aren't doing proper error checking, but I've added some for this new code in 2e9f08e


TiXmlElementPtr blob =
std::make_shared<TiXmlElement>(*xmlNewDoc.FirstChildElement());
sdfExt->blobs.push_back(blob);
};
stringToExtension(ssj.str());
stringToExtension(ssl.str());

// Ensure model extension vector is allocated
if (g_extensions.find("") == g_extensions.end())
{
std::vector<SDFExtensionPtr> ge;
g_extensions.insert(std::make_pair("", ge));
}

// Add //frame tags to model extension vector
g_extensions.at("").push_back(sdfExt);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit: This would do the same thing, I believe.

Suggested change
// Ensure model extension vector is allocated
if (g_extensions.find("") == g_extensions.end())
{
std::vector<SDFExtensionPtr> ge;
g_extensions.insert(std::make_pair("", ge));
}
// Add //frame tags to model extension vector
g_extensions.at("").push_back(sdfExt);
g_extensions[""].push_back(sdfExt);

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

good call: simplified in 2e9f08e


// lump sdf extensions to parent, (give them new reference _link names)
ReduceSDFExtensionToParent(_link);

Expand Down
59 changes: 59 additions & 0 deletions test/integration/fixed_joint_example.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
<?xml version="1.0" ?>
<robot name="fixed_joint_example">

<link name="base">
<inertial>
<mass value="6"/>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<inertia ixx="0.625" ixy="0" ixz="0" iyy="0.625" iyz="0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<box size="1.0 1.0 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
</visual>
</link>

<!-- Intermediate link with fixed joint that will be reduced. -->
<joint name="intermediate_joint" type="fixed">
<origin rpy="1.57079632679 0 0" xyz="0 0 0.75"/>
<parent link="base"/>
<child link="intermediate_link"/>
</joint>
<link name="intermediate_link">
<inertial>
<mass value="6"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.625" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="0.625"/>
</inertial>
<visual>
<geometry>
<box size="1.0 0.5 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
</link>

<!-- Continuous joint connected to intermediate_link -->
<joint name="rotary_joint" type="continuous">
<origin rpy="0 0 1.57079632679" xyz="0 0.75 0"/>
<axis xyz="1 0 0"/>
<parent link="intermediate_link"/>
<child link="rotary_link"/>
</joint>
<link name="rotary_link">
<inertial>
<mass value="12"/>
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you add a comment that this <inertial> is set with the expected value of the <inerital> of base and intermediate_link combined?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I added some clarifying comments about the model in 2e9f08e, including what you have suggested here

<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="2" ixy="0" ixz="0" iyy="2" iyz="0" izz="2"/>
</inertial>
<visual>
<geometry>
<box size="1.0 1.0 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
</link>

</robot>
34 changes: 34 additions & 0 deletions test/integration/fixed_joint_simple.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
<?xml version="1.0" ?>
<robot name="fixed_joint_simple">

<link name="base">
<inertial>
<mass value="1"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
</link>

<!-- Intermediate fixed joint that will be reduced. -->
<joint name="fixed_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="child_link"/>
</joint>

<!-- Child link with visual that will be merged to base link -->
<link name="child_link">
<inertial>
<mass value="1"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1"/>
</inertial>
<visual>
<geometry>
<box size="1.0 1.0 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
</link>

</robot>
115 changes: 111 additions & 4 deletions test/integration/urdf_gazebo_extensions.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,11 +29,16 @@ 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);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add an assertion on modelDom.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done in 2e9f08e

sdf::ElementPtr model = modelDom->Element();
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
joint = joint->GetNextElement("joint"))
{
Expand Down Expand Up @@ -306,4 +311,106 @@ 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"));

// Expect MassMatrix3 values to match for links
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit: remove?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done in 2e9f08e

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());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit : Can this be changed to gz namespace ? I guess not, since this is targeting an older sdf branch.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it would be possible, but we would also have to bump the minimum required version of ignition-math6 to 6.13.0. These changes are already being handled in #1118, and it's slightly simpler for VIPER if we can merge this PR without the gz:: namespaces, so I'd prefer to defer that change for now


EXPECT_EQ(0u, model->JointCount());

EXPECT_EQ(2u, model->FrameCount());
ASSERT_TRUE(model->FrameNameExists("fixed_joint"));
ASSERT_TRUE(model->FrameNameExists("child_link"));
}