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

Fix static URDF models with fixed joints #1193

Merged
merged 4 commits into from
Nov 5, 2022
Merged

Fix static URDF models with fixed joints #1193

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
2dc5c1f
Add test for static URDF model with fixed joints
scpeters Nov 3, 2022
6580927
SDFExtension: add isSetStaticFlag variable
scpeters Nov 3, 2022
985f38a
Improve coverage
scpeters Nov 3, 2022
7ca2b38
SDFExtension: follow rule of zero
scpeters Nov 5, 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
2 changes: 2 additions & 0 deletions src/SDFExtension.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ SDFExtension::SDFExtension()
this->material.clear();
this->visual_blobs.clear();
this->collision_blobs.clear();
this->isSetStaticFlag = false;
this->setStaticFlag = false;
this->isGravity = false;
this->gravity = true;
Expand Down Expand Up @@ -74,6 +75,7 @@ SDFExtension::SDFExtension(const SDFExtension &_ge)
this->material = _ge.material;
this->visual_blobs = _ge.visual_blobs;
this->collision_blobs = _ge.collision_blobs;
this->isSetStaticFlag = _ge.isSetStaticFlag;
Copy link
Contributor

Choose a reason for hiding this comment

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

Can we do something about this line in codecov ?

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 just removed the SDFExtension copy constructor and destructor so the class can follow the rule of zero. That should improve coverage.

Copy link
Member Author

Choose a reason for hiding this comment

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

coverage checker is green; removed 45 missed lines

this->setStaticFlag = _ge.setStaticFlag;
this->isGravity = _ge.isGravity;
this->gravity = _ge.gravity;
Expand Down
1 change: 1 addition & 0 deletions src/SDFExtension.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ namespace sdf
public: std::vector<std::shared_ptr<TiXmlElement> > collision_blobs;

// body, default off
public: bool isSetStaticFlag;
public: bool setStaticFlag;
public: bool isGravity;
public: bool gravity;
Expand Down
18 changes: 11 additions & 7 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1349,6 +1349,7 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml)
else if (childElem->ValueStr() == "static")
{
std::string valueStr = GetKeyValueAsString(childElem);
sdf->isSetStaticFlag = true;

// default of setting static flag is false
if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
Expand Down Expand Up @@ -2284,14 +2285,17 @@ void InsertSDFExtensionRobot(TiXmlElement *_elem)
for (std::vector<SDFExtensionPtr>::iterator
ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge)
{
// insert static flag
if ((*ge)->setStaticFlag)
if ((*ge)->isSetStaticFlag)
{
AddKeyValue(_elem, "static", "true");
}
else
{
AddKeyValue(_elem, "static", "false");
// insert static flag
if ((*ge)->setStaticFlag)
{
AddKeyValue(_elem, "static", "true");
}
else
{
AddKeyValue(_elem, "static", "false");
}
}

// copy extension containing blobs and without reference
Expand Down
38 changes: 38 additions & 0 deletions test/integration/fixed_joint_static.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
<?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>

<gazebo>
<static>1</static>
</gazebo>

</robot>
41 changes: 41 additions & 0 deletions test/integration/urdf_gazebo_extensions.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)

auto modelDom = root.ModelByIndex(0);
ASSERT_NE(nullptr, modelDom);

// Verify that model is not static
EXPECT_FALSE(modelDom->Static());

sdf::ElementPtr model = modelDom->Element();
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
joint = joint->GetNextElement("joint"))
Expand Down Expand Up @@ -398,6 +402,43 @@ TEST(SDFParser, FixedJointSimple)
auto model = root.ModelByIndex(0);
ASSERT_NE(nullptr, model);
EXPECT_EQ("fixed_joint_simple", model->Name());
EXPECT_FALSE(model->Static());

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"));
}

/////////////////////////////////////////////////
TEST(SDFParser, FixedJointStatic)
{
const std::string urdfTestFile =
sdf::testing::TestFile("integration", "fixed_joint_static.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_TRUE(model->Static());

EXPECT_EQ(1u, model->LinkCount());
EXPECT_TRUE(model->LinkNameExists("base"));
Expand Down
5 changes: 5 additions & 0 deletions test/integration/urdf_gazebo_extensions.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
<?xml version="1.0" ?>
<robot name="urdf_gazebo_extension_test">

<gazebo>
<!-- Explicitly declare that model is not static. -->
<static>0</static>
</gazebo>

<link name="world"/>

<joint name="jointw0" type="continuous">
Expand Down