Skip to content

Commit

Permalink
Rename CalculateInertials() API in Root, World, Model & Link
Browse files Browse the repository at this point in the history
Signed-off-by: Jasmeet Singh <[email protected]>
  • Loading branch information
jasmeet0915 committed Aug 30, 2023
1 parent 91c828a commit 1986cfd
Show file tree
Hide file tree
Showing 18 changed files with 51 additions and 51 deletions.
2 changes: 1 addition & 1 deletion include/sdf/Link.hh
Original file line number Diff line number Diff line change
Expand Up @@ -335,7 +335,7 @@ namespace sdf
/// \param[out] _errors A vector of Errors object. Each object
/// would contain an error code and an error message.
/// \param[in] _config Custom parser configuration
public: void CalculateInertials(sdf::Errors &_errors,
public: void ResolveAutoInertials(sdf::Errors &_errors,
const ParserConfig &_config);

/// \brief Get the pose of the link. This is the pose of the link
Expand Down
2 changes: 1 addition & 1 deletion include/sdf/Model.hh
Original file line number Diff line number Diff line change
Expand Up @@ -513,7 +513,7 @@ namespace sdf
/// \param[out] _errrors A vector of Errors objects. Each errors contains an
/// Error code and a message. An empty errors vector indicates no errors
/// \param[in] _config Custom parser configuration
public: void CalculateInertials(sdf::Errors &_errors,
public: void ResolveAutoInertials(sdf::Errors &_errors,
const ParserConfig &_config);

/// \brief Give the scoped PoseRelativeToGraph to be used for resolving
Expand Down
10 changes: 5 additions & 5 deletions include/sdf/ParserConfig.hh
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,10 @@ enum class EnforcementPolicy
LOG,
};

/// \enum ConfigureCalculateInertial
/// \enum ConfigureResolveAutoInertials
/// \brief Configuration options of how CalculateInertial() function
/// would be used
enum class ConfigureCalculateInertial
enum class ConfigureResolveAutoInertials
{
/// \brief If this value is used, CalculateInertial() won't be
/// called from inside the Root::Load() function
Expand Down Expand Up @@ -176,14 +176,14 @@ class SDFORMAT_VISIBLE ParserConfig

/// \brief Get the current configuration for the CalculateInertial()
/// function
/// \return Current set value of the ConfigureCalculateInertial enum
public: ConfigureCalculateInertial CalculateInertialConfiguration() const;
/// \return Current set value of the ConfigureResolveAutoInertials enum
public: ConfigureResolveAutoInertials CalculateInertialConfiguration() const;

/// \brief Set the configuration for the CalculateInertial() function
/// \param[in] _configuration The configuration to set for the
/// CalculateInertial() function
public: void SetCalculateInertialConfiguration(
ConfigureCalculateInertial _configuration);
ConfigureResolveAutoInertials _configuration);

/// \brief Registers a custom model parser.
/// \param[in] _modelParser Callback as described in
Expand Down
2 changes: 1 addition & 1 deletion include/sdf/Root.hh
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,7 @@ namespace sdf
/// \param[out] _errors A vector of Errors objects. Each errors contains an
/// Error code and a message. An empty errors vector indicates no errors
/// \param[in] _config Custom parser configuration
public: void CalculateInertials(sdf::Errors &_errors,
public: void ResolveAutoInertials(sdf::Errors &_errors,
const ParserConfig &_config);

/// \brief Create and return an SDF element filled with data from this
Expand Down
2 changes: 1 addition & 1 deletion include/sdf/World.hh
Original file line number Diff line number Diff line change
Expand Up @@ -521,7 +521,7 @@ namespace sdf
/// \param[out] _errrors A vector of Errors objects. Each errors contains an
/// Error code and a message. An empty errors vector indicates no errors
/// \param[in] _config Custom parser configuration
public: void CalculateInertials(sdf::Errors &_errors,
public: void ResolveAutoInertials(sdf::Errors &_errors,
const ParserConfig &_config);

/// \brief Give the Scoped PoseRelativeToGraph to be passed on to child
Expand Down
4 changes: 2 additions & 2 deletions src/Box_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -158,8 +158,8 @@ TEST(DOMBox, CalculateInertial)
// density of aluminium
double density = 2710;

// Invalid dimensions leading to std::nullopt return
// CalculateInertials()
// Invalid dimensions leading to std::nullopt return in
// CalculateInertial()
box.SetSize(gz::math::Vector3d(-1, 1, 0));
auto invalidBoxInertial = box.CalculateInertial(density);
ASSERT_EQ(std::nullopt, invalidBoxInertial);
Expand Down
4 changes: 2 additions & 2 deletions src/Collision_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -242,7 +242,7 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial)
const sdf::Collision *collision = link->CollisionByIndex(0);

sdf::Errors inertialErr;
root.CalculateInertials(inertialErr, sdfParserConfig);
root.ResolveAutoInertials(inertialErr, sdfParserConfig);

const double l = 2;
const double w = 2;
Expand Down Expand Up @@ -305,7 +305,7 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink)
const sdf::Collision *collision = link->CollisionByIndex(0);

sdf::Errors inertialErr;
root.CalculateInertials(inertialErr, sdfParserConfig);
root.ResolveAutoInertials(inertialErr, sdfParserConfig);

const double l = 2;
const double w = 2;
Expand Down
4 changes: 2 additions & 2 deletions src/Cylinder_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -190,8 +190,8 @@ TEST(DOMCylinder, CalculateInertial)
// density of aluminium
const double density = 2170;

// Invalid dimensions leading to std::nullopt return
// CalculateInertials()
// Invalid dimensions leading to std::nullopt return in
// CalculateInertial()
cylinder.SetLength(-1);
cylinder.SetRadius(0);
auto invalidCylinderInertial = cylinder.CalculateInertial(density);
Expand Down
4 changes: 2 additions & 2 deletions src/Ellipsoid_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -154,8 +154,8 @@ TEST(DOMEllipsoid, CalculateInertial)
// density of aluminium
const double density = 2170;

// Invalid dimensions leading to std::nullopt return
// CalculateInertials()
// Invalid dimensions leading to std::nullopt return in
// CalculateInertial()
ellipsoid.SetRadii(gz::math::Vector3d(-1, 2, 0));
auto invalidEllipsoidInertial = ellipsoid.CalculateInertial(density);
ASSERT_EQ(std::nullopt, invalidEllipsoidInertial);
Expand Down
4 changes: 2 additions & 2 deletions src/Link.cc
Original file line number Diff line number Diff line change
Expand Up @@ -597,7 +597,7 @@ Errors Link::ResolveInertial(
}

/////////////////////////////////////////////////
void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config)
void Link::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config)
{
// If inertial calculations is set to automatic & the inertial values for the
// link was not saved previously
Expand Down Expand Up @@ -627,7 +627,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config)
// If CalculateInertial() was called with SAVE_CALCULATION
// configuration then set autoInertiaSaved to true
if (_config.CalculateInertialConfiguration() ==
ConfigureCalculateInertial::SAVE_CALCULATION)
ConfigureResolveAutoInertials::SAVE_CALCULATION)
{
this->dataPtr->autoInertiaSaved = true;
}
Expand Down
14 changes: 7 additions & 7 deletions src/Link_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -279,7 +279,7 @@ TEST(DOMLink, InvalidInertia)
}

/////////////////////////////////////////////////
TEST(DOMLink, CalculateInertialWithNoCollisionsInLink)
TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink)
{
std::string sdf = "<?xml version=\"1.0\"?>"
" <sdf version=\"1.11\">"
Expand All @@ -298,7 +298,7 @@ TEST(DOMLink, CalculateInertialWithNoCollisionsInLink)

const sdf::Model *model = root.Model();
const sdf::Link *link = model->LinkByIndex(0);
root.CalculateInertials(errors, sdfParserConfig);
root.ResolveAutoInertials(errors, sdfParserConfig);
ASSERT_FALSE(errors.empty());

// Default Inertial values set during load
Expand All @@ -308,7 +308,7 @@ TEST(DOMLink, CalculateInertialWithNoCollisionsInLink)
}

/////////////////////////////////////////////////
TEST(DOMLink, CalculateInertialWithMultipleCollisions)
TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions)
{
std::string sdf = "<?xml version=\"1.0\"?>"
"<sdf version=\"1.11\">"
Expand Down Expand Up @@ -347,7 +347,7 @@ TEST(DOMLink, CalculateInertialWithMultipleCollisions)

const sdf::Model *model = root.Model();
const sdf::Link *link = model->LinkByIndex(0);
root.CalculateInertials(errors, sdfParserConfig);
root.ResolveAutoInertials(errors, sdfParserConfig);
EXPECT_TRUE(errors.empty());

// Mass of cube(volume * density) + mass of cylinder(volume * density)
Expand Down Expand Up @@ -394,7 +394,7 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)

const sdf::Model *model = root.Model();
const sdf::Link *link = model->LinkByIndex(0);
root.CalculateInertials(errors, sdfParserConfig);
root.ResolveAutoInertials(errors, sdfParserConfig);
EXPECT_TRUE(errors.empty());

EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass());
Expand All @@ -404,7 +404,7 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)
}

/////////////////////////////////////////////////
TEST(DOMLink, CalculateInertialCalledWithAutoFalse)
TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse)
{
std::string sdf = "<?xml version=\"1.0\"?>"
" <sdf version=\"1.11\">"
Expand All @@ -423,7 +423,7 @@ TEST(DOMLink, CalculateInertialCalledWithAutoFalse)

const sdf::Model *model = root.Model();
const sdf::Link *link = model->LinkByIndex(0);
root.CalculateInertials(errors, sdfParserConfig);
root.ResolveAutoInertials(errors, sdfParserConfig);
EXPECT_TRUE(errors.empty());

// Default Inertial values set during load
Expand Down
6 changes: 3 additions & 3 deletions src/Model.cc
Original file line number Diff line number Diff line change
Expand Up @@ -906,19 +906,19 @@ void Model::SetPoseRelativeTo(const std::string &_frame)
}

/////////////////////////////////////////////////
void Model::CalculateInertials(sdf::Errors &_errors,
void Model::ResolveAutoInertials(sdf::Errors &_errors,
const ParserConfig &_config)
{
// Loop through all the nested models, if there are any
for (sdf::Model &model : this->dataPtr->models)
{
model.CalculateInertials(_errors, _config);
model.ResolveAutoInertials(_errors, _config);
}

// Calculate and set inertials for all the links in the model
for (sdf::Link &link : this->dataPtr->links)
{
link.CalculateInertials(_errors, _config);
link.ResolveAutoInertials(_errors, _config);
}
}

Expand Down
12 changes: 6 additions & 6 deletions src/ParserConfig.cc
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,8 @@ class sdf::ParserConfig::Implementation
/// \brief Configuration that is set for the CalculateInertial() function
/// By default it is set to SKIP_CALCULATION_IN_LOAD which would cause
/// Root::Load() to not call CalculateInertial()
public: ConfigureCalculateInertial calculateInertialConfiguration =
ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD;
public: ConfigureResolveAutoInertials resolveAutoInertialsConfig =
ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD;

/// \brief Collection of custom model parsers.
public: std::vector<CustomModelParser> customParsers;
Expand Down Expand Up @@ -164,16 +164,16 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const
}

/////////////////////////////////////////////////
ConfigureCalculateInertial ParserConfig::CalculateInertialConfiguration() const
ConfigureResolveAutoInertials ParserConfig::CalculateInertialConfiguration() const
{
return this->dataPtr->calculateInertialConfiguration;
return this->dataPtr->resolveAutoInertialsConfig;
}

/////////////////////////////////////////////////
void ParserConfig::SetCalculateInertialConfiguration(
ConfigureCalculateInertial _configuration)
ConfigureResolveAutoInertials _configuration)
{
this->dataPtr->calculateInertialConfiguration = _configuration;
this->dataPtr->resolveAutoInertialsConfig = _configuration;
}

/////////////////////////////////////////////////
Expand Down
6 changes: 3 additions & 3 deletions src/ParserConfig_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,11 @@ TEST(ParserConfig, Construction)
EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy());
EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy());

EXPECT_EQ(sdf::ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD,
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
config.CalculateInertialConfiguration());
config.SetCalculateInertialConfiguration(
sdf::ConfigureCalculateInertial::SAVE_CALCULATION);
EXPECT_EQ(sdf::ConfigureCalculateInertial::SAVE_CALCULATION,
sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION);
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION,
config.CalculateInertialConfiguration());

EXPECT_FALSE(config.URDFPreserveFixedJoint());
Expand Down
10 changes: 5 additions & 5 deletions src/Root.cc
Original file line number Diff line number Diff line change
Expand Up @@ -394,9 +394,9 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config)

// Check if CalculateInertialConfiguration() is not set to skip in load
if (_config.CalculateInertialConfiguration() !=
ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD)
ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD)
{
this->CalculateInertials(errors, _config);
this->ResolveAutoInertials(errors, _config);
}

return errors;
Expand Down Expand Up @@ -598,19 +598,19 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model,
}

/////////////////////////////////////////////////
void Root::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config)
void Root::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config)
{
// Calculate and set Inertials for all the worlds
for (sdf::World &world : this->dataPtr->worlds)
{
world.CalculateInertials(_errors, _config);
world.ResolveAutoInertials(_errors, _config);
}

// Calculate and set Inertials for the model, if it is present
if (std::holds_alternative<sdf::Model>(this->dataPtr->modelLightOrActor))
{
sdf::Model &model = std::get<sdf::Model>(this->dataPtr->modelLightOrActor);
model.CalculateInertials(_errors, _config);
model.ResolveAutoInertials(_errors, _config);
}
}

Expand Down
6 changes: 3 additions & 3 deletions src/Root_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -325,7 +325,7 @@ TEST(DOMRoot, FrameSemanticsOnMove)
}

/////////////////////////////////////////////////
TEST(DOMRoot, CalculateInertialWithSaveCalculationConfiguration)
TEST(DOMRoot, ResolveAutoInertialsWithSaveCalculationConfiguration)
{
std::string sdf = "<?xml version=\"1.0\"?>"
" <sdf version=\"1.11\">"
Expand Down Expand Up @@ -354,10 +354,10 @@ TEST(DOMRoot, CalculateInertialWithSaveCalculationConfiguration)
const sdf::Link *link = model->LinkByIndex(0);

sdfParserConfig.SetCalculateInertialConfiguration(
sdf::ConfigureCalculateInertial::SAVE_CALCULATION);
sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION);

sdf::Errors inertialErr;
root.CalculateInertials(inertialErr, sdfParserConfig);
root.ResolveAutoInertials(inertialErr, sdfParserConfig);
EXPECT_TRUE(inertialErr.empty());
ASSERT_TRUE(link->AutoInertiaSaved());
}
Expand Down
6 changes: 3 additions & 3 deletions src/World.cc
Original file line number Diff line number Diff line change
Expand Up @@ -860,13 +860,13 @@ const NestedInclude *World::InterfaceModelNestedIncludeByIndex(
}

/////////////////////////////////////////////////
void World::CalculateInertials(sdf::Errors &_errors,
void World::ResolveAutoInertials(sdf::Errors &_errors,
const ParserConfig &_config)
{
// Call CalculateInertials() function for all the models
// Call ResolveAutoInertials() function for all the models
for (sdf::Model &model : this->dataPtr->models)
{
model.CalculateInertials(_errors, _config);
model.ResolveAutoInertials(_errors, _config);
}
}

Expand Down
4 changes: 2 additions & 2 deletions src/World_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -516,7 +516,7 @@ TEST(DOMWorld, AddLight)
}

/////////////////////////////////////////////////
TEST(DOMWorld, CalculateInertial)
TEST(DOMWorld, ResolveAutoInertials)
{
std::string sdf = "<?xml version=\"1.0\"?>"
" <sdf version=\"1.11\">"
Expand Down Expand Up @@ -547,7 +547,7 @@ TEST(DOMWorld, CalculateInertial)
const sdf::Model *model = world->ModelByIndex(0);
const sdf::Link *link = model->LinkByIndex(0);

root.CalculateInertials(errors, sdfParserConfig);
root.ResolveAutoInertials(errors, sdfParserConfig);

const double l = 2;
const double w = 2;
Expand Down

0 comments on commit 1986cfd

Please sign in to comment.