Skip to content

Commit

Permalink
Use //link/inertial/density for auto-inertials
Browse files Browse the repository at this point in the history 
The density precedence for Collisions is now:
1. Density explicitly set in //collision/density
   or by Collision::SetDensity.
2. Density explicitly set in //link/inertial/density
   or by Link::SetDensity.
3. Default density in Collision::DensityDefault().

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters committed Oct 8, 2023
1 parent 013bc33 commit 92ca797
Show file tree
Hide file tree
Showing 7 changed files with 254 additions and 13 deletions.
22 changes: 21 additions & 1 deletion include/sdf/Collision.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#define SDF_COLLISION_HH_

#include <memory>
#include <optional>
#include <string>
#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
Expand Down Expand Up @@ -78,6 +79,11 @@ namespace sdf
/// \param[in] _name Name of the collision.
public: void SetName(const std::string &_name);

/// \brief Get the default density of a collision if its density is not
/// specified.
/// \return Default density.
public: static double DensityDefault();

/// \brief Get the density of the collision.
/// \return Density of the collision.
public: double Density() const;
Expand Down Expand Up @@ -145,13 +151,27 @@ namespace sdf
/// \brief Calculate and return the MassMatrix for the collision
/// \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
/// \param[out] _inertial An inertial object which will be set with the
/// \param[in] _config Custom parser configuration
/// calculated inertial values
public: void CalculateInertial(sdf::Errors &_errors,
gz::math::Inertiald &_inertial,
const ParserConfig &_config);

/// \brief Calculate and return the MassMatrix for the collision
/// \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[out] _inertial An inertial object which will be set with the
/// calculated inertial values
/// \param[in] _config Custom parser configuration
/// \param[in] _density An optional density value to override this
/// collision's density.
public: void CalculateInertial(
sdf::Errors &_errors,
gz::math::Inertiald &_inertial,
const ParserConfig &_config,
const std::optional<double> &_density);

/// \brief Get a pointer to the SDF element that was used during
/// load.
/// \return SDF element pointer. The value will be nullptr if Load has
Expand Down
10 changes: 10 additions & 0 deletions include/sdf/Link.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#define SDF_LINK_HH_

#include <memory>
#include <optional>
#include <string>
#include <gz/math/Inertial.hh>
#include <gz/math/Pose3.hh>
Expand Down Expand Up @@ -79,6 +80,15 @@ namespace sdf
/// \param[in] _name Name of the link.
public: void SetName(const std::string &_name);

/// \brief Get the density of the inertial if it has been set.
/// \return Density of the inertial if it has been set,
/// otherwise std::nullopt.
public: std::optional<double> Density() const;

/// \brief Set the density of the inertial.
/// \param[in] _density Density of the inertial.
public: void SetDensity(double _density);

/// \brief Get the number of visuals.
/// \return Number of visuals contained in this Link object.
public: uint64_t VisualCount() const;
Expand Down
54 changes: 45 additions & 9 deletions src/Collision.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,8 @@ class sdf::Collision::Implementation
/// \brief The collision's surface parameters.
public: sdf::Surface surface;

/// \brief Density of the collision. Default is 1000.0
public: double density{1000.0};
/// \brief Density of the collision if it has been set.
public: std::optional<double> density;

/// \brief True if density was set during load from sdf.
public: bool densitySetAtLoad = false;
Expand Down Expand Up @@ -130,7 +130,6 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config)
if (_sdf->HasElement("density"))
{
this->dataPtr->density = _sdf->Get<double>("density");
this->dataPtr->densitySetAtLoad = true;
}

// Load the auto_inertia_params element
Expand All @@ -155,10 +154,20 @@ void Collision::SetName(const std::string &_name)
this->dataPtr->name = _name;
}

/////////////////////////////////////////////////
double Collision::DensityDefault()
{
return 1000.0;
}

/////////////////////////////////////////////////
double Collision::Density() const
{
return this->dataPtr->density;
if (this->dataPtr->density)
{
return *this->dataPtr->density;
}
return DensityDefault();
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -256,14 +265,41 @@ void Collision::CalculateInertial(
gz::math::Inertiald &_inertial,
const ParserConfig &_config)
{
// Check if density was not set during load & send a warning
// about the default value being used
if (!this->dataPtr->densitySetAtLoad)
this->CalculateInertial(_errors, _inertial, _config, std::nullopt);
}

/////////////////////////////////////////////////
void Collision::CalculateInertial(
sdf::Errors &_errors,
gz::math::Inertiald &_inertial,
const ParserConfig &_config,
const std::optional<double> &_density)
{
// Order of precedence for density:
double density;
// 1. Density explicitly set in this collision, either from the
// `//collision/density` element or from Collision::SetDensity.
if (this->dataPtr->density)
{
density = *this->dataPtr->density;
}
// 2. Density passed into this function, which likely comes from the
// `//link/inertial/density` element or from Link::SetDensity.
else if (_density)
{
density = *_density;
}
// 3. DensityDefault value.
else
{
// If density was not explicitly set, send a warning
// about the default value being used
density = DensityDefault();
Error densityMissingErr(
ErrorCode::ELEMENT_MISSING,
"Collision is missing a <density> child element. "
"Using a default density value of 1000.0 kg/m^3. "
"Using a default density value of " +
std::to_string(DensityDefault()) + " kg/m^3. "
);
enforceConfigurablePolicyCondition(
_config.WarningsPolicy(), densityMissingErr, _errors
Expand All @@ -272,7 +308,7 @@ void Collision::CalculateInertial(

auto geomInertial =
this->dataPtr->geom.CalculateInertial(_errors, _config,
this->dataPtr->density, this->dataPtr->autoInertiaParams);
density, this->dataPtr->autoInertiaParams);

if (!geomInertial)
{
Expand Down
1 change: 1 addition & 0 deletions src/Collision_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ TEST(DOMcollision, Construction)
EXPECT_EQ(nullptr, collision.Element());
EXPECT_TRUE(collision.Name().empty());
EXPECT_EQ(collision.Density(), 1000.0);
EXPECT_EQ(collision.DensityDefault(), 1000.0);

collision.SetName("test_collison");
EXPECT_EQ(collision.Name(), "test_collison");
Expand Down
24 changes: 23 additions & 1 deletion src/Link.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,10 @@ class sdf::Link::Implementation
/// \brief The projectors specified in this link.
public: std::vector<Projector> projectors;

/// \brief Density of the inertial which will be used for auto-inertial
/// calculations if the collision density has not been set.
public: std::optional<double> density;

/// \brief The inertial information for this link.
public: gz::math::Inertiald inertial {{1.0,
gz::math::Vector3d::One, gz::math::Vector3d::Zero},
Expand Down Expand Up @@ -180,6 +184,11 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
{
sdf::ElementPtr inertialElem = _sdf->GetElement("inertial");

if (inertialElem->HasElement("density"))
{
this->dataPtr->density = inertialElem->Get<double>("density");
}

if (inertialElem->Get<bool>("auto"))
{
this->dataPtr->autoInertia = true;
Expand Down Expand Up @@ -309,6 +318,18 @@ void Link::SetName(const std::string &_name)
this->dataPtr->name = _name;
}

/////////////////////////////////////////////////
std::optional<double> Link::Density() const
{
return this->dataPtr->density;
}

/////////////////////////////////////////////////
void Link::SetDensity(double _density)
{
this->dataPtr->density = _density;
}

/////////////////////////////////////////////////
uint64_t Link::VisualCount() const
{
Expand Down Expand Up @@ -620,7 +641,8 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
for (sdf::Collision &collision : this->dataPtr->collisions)
{
gz::math::Inertiald collisionInertia;
collision.CalculateInertial(_errors, collisionInertia, _config);
collision.CalculateInertial(_errors, collisionInertia, _config,
this->dataPtr->density);
totalInertia = totalInertia + collisionInertia;
}

Expand Down
151 changes: 151 additions & 0 deletions src/Link_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,8 @@ TEST(DOMLink, Construction)
EXPECT_EQ(inertial.BodyMatrix(), inertial.SpatialMatrix());
EXPECT_TRUE(inertial.MassMatrix().IsValid());

EXPECT_FALSE(link.Density().has_value());

EXPECT_EQ(0u, link.CollisionCount());
EXPECT_EQ(nullptr, link.CollisionByIndex(0));
EXPECT_EQ(nullptr, link.CollisionByIndex(1));
Expand Down Expand Up @@ -302,6 +304,155 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink)
EXPECT_NE(nullptr, root.Element());
}

/////////////////////////////////////////////////
TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity)
{
std::string sdf = "<?xml version=\"1.0\"?>"
"<sdf version=\"1.11\">"
" <model name='multilink_model'>"
" <pose>0 0 1.0 0 0 0</pose>"
" <link name='link_density'>"
" <pose>0 1.0 0 0 0 0</pose>"
" <inertial auto='true'>"
" <density>12.0</density>"
" </inertial>"
" <collision name='box_collision'>"
" <pose>1.0 0 0 0 0 0</pose>"
" <geometry>"
" <box>"
" <size>1 1 1</size>"
" </box>"
" </geometry>"
" </collision>"
" </link>"
" <link name='collision_density'>"
" <pose>0 2.0 0 0 0 0</pose>"
" <inertial auto='true'/>"
" <collision name='box_collision'>"
" <pose>1.0 0 0 0 0 0</pose>"
" <density>24.0</density>"
" <geometry>"
" <box>"
" <size>1 1 1</size>"
" </box>"
" </geometry>"
" </collision>"
" </link>"
" <link name='collision_density_overrides_link_density'>"
" <pose>0 3.0 0 0 0 0</pose>"
" <inertial auto='true'>"
" <density>12.0</density>"
" </inertial>"
" <collision name='box_collision'>"
" <pose>1.0 0 0 0 0 0</pose>"
" <density>24.0</density>"
" <geometry>"
" <box>"
" <size>1 1 1</size>"
" </box>"
" </geometry>"
" </collision>"
" </link>"
" <link name='default_density'>"
" <pose>0 4.0 0 0 0 0</pose>"
" <inertial auto='true'/>"
" <collision name='box_collision'>"
" <pose>1.0 0 0 0 0 0</pose>"
" <geometry>"
" <box>"
" <size>1 1 1</size>"
" </box>"
" </geometry>"
" </collision>"
" </link>"
" </model>"
"</sdf>";

sdf::Root root;
const sdf::ParserConfig sdfParserConfig;
sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
EXPECT_TRUE(errors.empty()) << errors;
EXPECT_NE(nullptr, root.Element());

const sdf::Model *model = root.Model();
ASSERT_NE(nullptr, model);
EXPECT_EQ(4u, model->LinkCount());

// ResolveAutoInertials should run by default during Root::Load.

{
const sdf::Link *link = model->LinkByName("link_density");
ASSERT_NE(nullptr, link);
auto linkDensity = link->Density();
ASSERT_TRUE(linkDensity.has_value());
EXPECT_DOUBLE_EQ(12.0, *linkDensity);
// Expected inertial
auto inertial = link->Inertial();
EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose());
// box is 1 m^3, so expected mass == density
EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass());
EXPECT_EQ(gz::math::Vector3d(2, 2, 2),
inertial.MassMatrix().DiagonalMoments());
EXPECT_EQ(gz::math::Vector3d::Zero,
inertial.MassMatrix().OffDiagonalMoments());
}

{
const sdf::Link *link = model->LinkByName("collision_density");
ASSERT_NE(nullptr, link);
auto linkDensity = link->Density();
EXPECT_FALSE(linkDensity.has_value());
// assume Collision density is properly set to 24
// Expected inertial
auto inertial = link->Inertial();
EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose());
// box is 1 m^3, so expected mass == density
EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass());
EXPECT_EQ(gz::math::Vector3d(4, 4, 4),
inertial.MassMatrix().DiagonalMoments());
EXPECT_EQ(gz::math::Vector3d::Zero,
inertial.MassMatrix().OffDiagonalMoments());
}

{
const sdf::Link *link =
model->LinkByName("collision_density_overrides_link_density");
ASSERT_NE(nullptr, link);
auto linkDensity = link->Density();
ASSERT_TRUE(linkDensity.has_value());
EXPECT_DOUBLE_EQ(12.0, *linkDensity);
// assume Collision density is properly set to 24 and overrides the link
// density
// Expected inertial
auto inertial = link->Inertial();
EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose());
// box is 1 m^3, so expected mass == density
EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass());
EXPECT_EQ(gz::math::Vector3d(4, 4, 4),
inertial.MassMatrix().DiagonalMoments());
EXPECT_EQ(gz::math::Vector3d::Zero,
inertial.MassMatrix().OffDiagonalMoments());
}

{
const sdf::Link *link =
model->LinkByName("default_density");
ASSERT_NE(nullptr, link);
auto linkDensity = link->Density();
EXPECT_FALSE(linkDensity.has_value());
// assume density is the default value of 1000.0
// Expected inertial
auto inertial = link->Inertial();
EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose());
// box is 1 m^3, so expected mass == density
EXPECT_DOUBLE_EQ(1000.0, inertial.MassMatrix().Mass());
EXPECT_EQ(gz::math::Vector3d::One * 500.0 / 3.0,
inertial.MassMatrix().DiagonalMoments());
EXPECT_EQ(gz::math::Vector3d::Zero,
inertial.MassMatrix().OffDiagonalMoments());
}
}

/////////////////////////////////////////////////
TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions)
{
Expand Down
Loading

0 comments on commit 92ca797

Please sign in to comment.