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

Print auto inertial values with gz sdf --print --expand-auto-inertials #1422

Merged
merged 8 commits into from
May 24, 2024
Merged

Print auto inertial values with gz sdf --print --expand-auto-inertials #1422

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
6380fb7
ParserConfig: support printing auto-inertial data
scpeters May 20, 2024
6b5b245
Link: store auto-computed inertial data in Element
scpeters May 20, 2024
b17da3f
cmdsdformat: clean up help string
scpeters May 20, 2024
25f2dea
gz sdf --print: add --expand-auto-inertials
scpeters May 20, 2024
2ffda3d
fix test
scpeters May 21, 2024
a69d819
Merge remote-tracking branch 'origin/main' into scpeters/print_auto_i…
scpeters May 21, 2024
f32393b
Clarify that mesh inertial data is not computed
scpeters May 24, 2024
82508a7
Merge remote-tracking branch 'origin/main' into scpeters/print_auto_i…
scpeters May 24, 2024
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
8 changes: 7 additions & 1 deletion include/sdf/ParserConfig.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,13 @@ enum class ConfigureResolveAutoInertials

/// \brief If this values is used, CalculateInertial() would be
/// called and the computed inertial values would be saved
SAVE_CALCULATION
SAVE_CALCULATION,

/// \brief If this values is used, CalculateInertial() would be
/// called and the computed inertial values would be saved and
/// written to the XML Element, allowing the calculated values
/// to be printed with `gz sdf --print`.
SAVE_CALCULATION_IN_ELEMENT,
};

// Forward declare private data class.
Expand Down
24 changes: 24 additions & 0 deletions src/Link.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -684,6 +684,30 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
{
this->dataPtr->autoInertiaSaved = true;
}
else if (_config.CalculateInertialConfiguration() ==
ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT)
{
this->dataPtr->autoInertiaSaved = true;
// Write calculated inertia values to //link/inertial element
auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
inertialElem->GetElement("pose")->GetValue()->Set<gz::math::Pose3d>(
totalInertia.Pose());
inertialElem->GetElement("mass")->GetValue()->Set<double>(
totalInertia.MassMatrix().Mass());
auto momentOfInertiaElem = inertialElem->GetElement("inertia");
momentOfInertiaElem->GetElement("ixx")->GetValue()->Set<double>(
totalInertia.MassMatrix().Ixx());
momentOfInertiaElem->GetElement("ixy")->GetValue()->Set<double>(
totalInertia.MassMatrix().Ixy());
momentOfInertiaElem->GetElement("ixz")->GetValue()->Set<double>(
totalInertia.MassMatrix().Ixz());
momentOfInertiaElem->GetElement("iyy")->GetValue()->Set<double>(
totalInertia.MassMatrix().Iyy());
momentOfInertiaElem->GetElement("iyz")->GetValue()->Set<double>(
totalInertia.MassMatrix().Iyz());
momentOfInertiaElem->GetElement("izz")->GetValue()->Set<double>(
totalInertia.MassMatrix().Izz());
}
}
// If auto is false, this means inertial values were set
// from user given values in Link::Load(), therefore we can return
Expand Down
5 changes: 5 additions & 0 deletions src/ParserConfig_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,11 @@ TEST(ParserConfig, Construction)
sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
config.CalculateInertialConfiguration());
config.SetCalculateInertialConfiguration(
sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT);
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT,
config.CalculateInertialConfiguration());

EXPECT_FALSE(config.URDFPreserveFixedJoint());
EXPECT_FALSE(config.StoreResolvedURIs());
}
Expand Down
12 changes: 9 additions & 3 deletions src/cmd/cmdsdformat.rb.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,16 +44,17 @@ COMMANDS = { 'sdf' =>
" -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" +
" -g [ --graph ] <pose, frame> arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" +
" use only and the output may change without any promise of stability)\n" +
" --inertial-stats arg Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" +
" -p [ --print ] arg Print converted arg. Note the quaternion representation of the\n" +
" rotational part of poses and unit vectors will be normalized.\n" +
" -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" +
" --degrees Pose rotation angles are printed in degrees.\n" +
" --expand-auto-inertials Prints auto-computed inertial values.\n" +
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 mention that this won't work for meshes.

Copy link
Member Author

Choose a reason for hiding this comment

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

in f32393b I've added a bit to the doc-string for gz sdf --print --expand-auto-inertials that mesh inertial data isn't computed. here's the output I see (with some adjusted whitespace):

$ gz sdf --print test/sdf/mesh_auto_inertial.sdf --expand-auto-inertials
Warning [Utils.cc:115] Collision is missing a <density> child element.
 Using a default density value of 1000.000000 kg/m^3.
Warning [Utils.cc:115] Custom moment of inertia calculator for meshes
 not set via sdf::ParserConfig::RegisterCustomInertiaCalc,
 using default inertial values.
<sdf version='1.12'>
  <model name='test_model'>
    <link name='L1'>
      <inertial auto='true'>
        <pose>0 0 0 0 0 0</pose>
        <mass>1</mass>
        <inertia>
          <ixx>1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1</iyy>
          <iyz>0</iyz>
          <izz>1</izz>
        </inertia>
      </inertial>
      <collision name='collision'>
        <geometry>
          <mesh>
            <uri>box.dae</uri>
          </mesh>
        </geometry>
      </collision>
    </link>
  </model>
</sdf>

" --snap-to-degrees arg Snap pose rotation angles to this specified interval in degrees. This value must be\n" +
" larger than 0, less than or equal to 360, and larger than the defined snap tolerance.\n" +
" --snap-tolerance arg Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" +
" occurs. This value must be larger than 0, less than 360, and less than the defined\n" +
" degrees value to snap to. If unspecified, its default value is 0.01.\n" +
" --inertial-stats arg Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" +
" --precision arg Set the output stream precision for floating point numbers. The arg must be a positive integer.\n" +

COMMON_OPTIONS
Expand All @@ -70,6 +71,7 @@ class Cmd
def parse(args)
options = {}
options['degrees'] = 0
options['expand_auto_inertials'] = 0
options['snap_tolerance'] = 0.01
options['preserve_includes'] = 0

Expand Down Expand Up @@ -104,6 +106,9 @@ class Cmd
opts.on('--degrees', 'Printed pose rotations are will be in degrees') do |degrees|
options['degrees'] = 1
end
opts.on('--expand-auto-inertials', 'Auto-computed inertial values will be printed') do
options['expand_auto_inertials'] = 1
end
opts.on('--snap-to-degrees arg', Integer,
'Printed rotations are snapped to specified degree intervals') do |arg|
if arg == 0 || arg > 360
Expand Down Expand Up @@ -243,13 +248,14 @@ class Cmd
if options.key?('precision')
precision = options['precision']
end
Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance, int, int)'
Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance, int, int, int)'
exit(Importer.cmdPrint(File.expand_path(options['print']),
options['degrees'],
snap_to_degrees,
options['snap_tolerance'],
options['preserve_includes'],
precision))
precision,
options['expand_auto_inertials']))
elsif options.key?('graph')
Importer.extern 'int cmdGraph(const char *, const char *)'
exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1])))
Expand Down
30 changes: 18 additions & 12 deletions src/gz.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include "sdf/Model.hh"
#include "sdf/Root.hh"
#include "sdf/parser.hh"
#include "sdf/ParserConfig.hh"
#include "sdf/PrintConfig.hh"
#include "sdf/system_util.hh"

Expand Down Expand Up @@ -140,27 +141,23 @@ extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version)
//////////////////////////////////////////////////
extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path,
int _inDegrees, int _snapToDegrees, float _snapTolerance,
int _preserveIncludes, int _outPrecision)
int _preserveIncludes, int _outPrecision, int _expandAutoInertials)
{
if (!sdf::filesystem::exists(_path))
{
std::cerr << "Error: File [" << _path << "] does not exist.\n";
return -1;
}

sdf::SDFPtr sdf(new sdf::SDF());

if (!sdf::init(sdf))
sdf::ParserConfig parserConfig;
if (_expandAutoInertials)
{
std::cerr << "Error: SDF schema initialization failed.\n";
return -1;
parserConfig.SetCalculateInertialConfiguration(
sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT);
}

if (!sdf::readFile(_path, sdf))
{
std::cerr << "Error: SDF parsing the xml failed.\n";
return -1;
}
sdf::Root root;
sdf::Errors errors = root.Load(_path, parserConfig);

sdf::PrintConfig config;
if (_inDegrees != 0)
Expand All @@ -180,7 +177,16 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path,
if (_outPrecision > 0)
config.SetOutPrecision(_outPrecision);

sdf->PrintValues(config);
if (root.Element())
{
root.Element()->PrintValues(errors, "", config);
}

if (!errors.empty())
{
std::cerr << errors << std::endl;
return -1;
}
return 0;
}

Expand Down
32 changes: 31 additions & 1 deletion src/gz_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1098,7 +1098,8 @@ TEST(print, SDF)
// Check box_bad_test.world
std::string output =
custom_exec_str(GzCommand() + " sdf -p " + path + SdfVersion());
EXPECT_TRUE(output.find("Required attribute") != std::string::npos);
EXPECT_TRUE(output.find("Required attribute") != std::string::npos)
<< output;
}
}

Expand Down Expand Up @@ -1726,6 +1727,35 @@ TEST(print_snap_to_degrees_tolerance_too_high, SDF)
"1 2 3 30 50 60</pose>");
}

/////////////////////////////////////////////////
TEST(print_auto_inertial, SDF)
{
const auto path = sdf::testing::TestFile("sdf", "inertial_stats_auto.sdf");

{
// Print without --expand-auto-inertials
// expect no <mass> or <inertia> elements
std::string output = custom_exec_str(
GzCommand() + " sdf -p " + path +
SdfVersion());
ASSERT_FALSE(output.empty());
EXPECT_PRED2(sdf::testing::notContains, output, "<mass>");
EXPECT_PRED2(sdf::testing::notContains, output, "<inertia>");
}

{
// Print with --expand-auto-inertials
// expect <mass> and <inertia> elements
std::string output = custom_exec_str(
GzCommand() + " sdf -p " + path +
" --expand-auto-inertials " +
SdfVersion());
ASSERT_FALSE(output.empty());
EXPECT_PRED2(sdf::testing::contains, output, "<mass>");
EXPECT_PRED2(sdf::testing::contains, output, "<inertia>");
}
}

/////////////////////////////////////////////////
TEST(GraphCmd, WorldPoseRelativeTo)
{
Expand Down
95 changes: 95 additions & 0 deletions test/integration/link_dom.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -930,3 +930,98 @@ TEST(DOMLink, InvalidInertialPoseRelTo)
EXPECT_EQ(link->Inertial().Pose(),
gz::math::Pose3d(0.1, 1, 0.2, 0, 0, -0.52));
}

/////////////////////////////////////////////////
TEST(DOMLink, InertialAuto)
{
const std::string testFile = sdf::testing::TestFile("sdf",
"inertial_stats_auto.sdf");

// Load the SDF file
sdf::Root root;
auto errors = root.Load(testFile);
EXPECT_TRUE(errors.empty()) << errors;

const sdf::Model *model = root.Model();
ASSERT_NE(model, nullptr);

const sdf::Link *link = model->LinkByName("link_1");
ASSERT_NE(link, nullptr);

// Verify inertial values for link_1 match thos in inertial_stats.sdf
gz::math::Inertiald inertial = link->Inertial();
gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
EXPECT_DOUBLE_EQ(6.0, massMatrix.Mass());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Ixx());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Iyy());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Izz());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixy());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixz());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Iyz());

// Expect //link/inertial element to not contain inertia, mass, or pose
auto linkElem = link->Element();
auto inertialElem = linkElem->FindElement("inertial");
ASSERT_TRUE(inertialElem != nullptr);
EXPECT_FALSE(inertialElem->HasElement("inertia"));
EXPECT_FALSE(inertialElem->HasElement("mass"));
EXPECT_FALSE(inertialElem->HasElement("pose"));
}

/////////////////////////////////////////////////
TEST(DOMLink, InertialAutoSaveInElement)
{
const std::string testFile = sdf::testing::TestFile("sdf",
"inertial_stats_auto.sdf");

// Load the SDF file with SAVE_CALCULATION_IN_ELEMENT
sdf::ParserConfig config;
config.SetCalculateInertialConfiguration(
sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT);

sdf::Root root;
auto errors = root.Load(testFile, config);
EXPECT_TRUE(errors.empty()) << errors;

const sdf::Model *model = root.Model();
ASSERT_NE(model, nullptr);

const sdf::Link *link = model->LinkByName("link_1");
ASSERT_NE(link, nullptr);

// Verify inertial values for link_1 match thos in inertial_stats.sdf
gz::math::Inertiald inertial = link->Inertial();
gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
EXPECT_DOUBLE_EQ(6.0, massMatrix.Mass());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Ixx());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Iyy());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Izz());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixy());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixz());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Iyz());

// Expect //link/inertial element to contain inertia values
auto linkElem = link->Element();
auto inertialElem = linkElem->FindElement("inertial");
ASSERT_TRUE(inertialElem != nullptr);
EXPECT_TRUE(inertialElem->HasElement("mass"));
EXPECT_DOUBLE_EQ(6.0, inertialElem->Get<double>("mass"));
EXPECT_EQ(gz::math::Pose3d::Zero,
inertialElem->Get<gz::math::Pose3d>("pose"));
auto momentOfInertiaElem = inertialElem->FindElement("inertia");
ASSERT_TRUE(momentOfInertiaElem != nullptr);
EXPECT_TRUE(momentOfInertiaElem->HasElement("ixx"));
EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get<double>("ixx"));
EXPECT_TRUE(momentOfInertiaElem->HasElement("iyy"));
EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get<double>("iyy"));
EXPECT_TRUE(momentOfInertiaElem->HasElement("izz"));
EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get<double>("izz"));
EXPECT_TRUE(momentOfInertiaElem->HasElement("ixy"));
EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get<double>("ixy"));
EXPECT_TRUE(momentOfInertiaElem->HasElement("ixz"));
EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get<double>("ixz"));
EXPECT_TRUE(momentOfInertiaElem->HasElement("iyz"));
EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get<double>("iyz"));
}
Loading