Skip to content

Commit 2d466af

Browse files
iche033iche033
committed
rescale inertia to match specified mass
Signed-off-by: Ian Chen <[email protected]> update python test Signed-off-by: Ian Chen <[email protected]> udate api doc for resolving auto inertia Signed-off-by: Ian Chen <[email protected]> Add tests for auto-inertial with explicit mass (#1514) * link_dom test: Fix typo Signed-off-by: Steve Peters <[email protected]> * Test case for auto-inertials with explicit mass Signed-off-by: Steve Peters <[email protected]> --------- Signed-off-by: Steve Peters <[email protected]> update doc add comments Signed-off-by: Ian Chen <[email protected]> add one more test with multiple collisions Signed-off-by: Ian Chen <[email protected]> Update mass expectation in auto-inertial tests The mass is no longer explicitly set in these Link tests, so replace the not-equals expectation with an expectation of what the mass should be. Signed-off-by: Steve Peters <[email protected]> fix build Signed-off-by: Ian Chen <[email protected]> remove empty line Signed-off-by: Ian Chen <[email protected]> remove density warning, fix typo Signed-off-by: Ian Chen <[email protected]> Update multiple-collision test with justification (#1515) Modifies the auto-inertial test with multiple collisions with different densities so that the lumped center of gravity is at the link origin and derives the expected moment of inertia values. Signed-off-by: Steve Peters <[email protected]>
1 parent ec757a6 commit 2d466af

File tree

8 files changed

+485
-23
lines changed

8 files changed

+485
-23
lines changed

‎include/sdf/Link.hh

+7-3
Original file line numberDiff line numberDiff line change
@@ -351,9 +351,13 @@ namespace sdf
351351
const std::string &_resolveTo = "") const;
352352

353353
/// \brief Calculate & set inertial values(mass, mass matrix
354-
/// & inertial pose) for the link. Inertial values can be provided
355-
/// by the user through the SDF or can be calculated automatically
356-
/// by setting the auto attribute to true.
354+
/// & inertial pose) for the link. This function will calculate
355+
/// the inertial values if the auto attribute is set to true.
356+
/// If mass is not provided by the user, the inertial values will be
357+
/// determined based on either link density or collision density if
358+
/// explicitly set. Otherwise, if mass is specified, the inertia matrix
359+
/// will be scaled to match the desired mass, while respecting
360+
/// the ratio of density values between collisions.
357361
/// \param[out] _errors A vector of Errors object. Each object
358362
/// would contain an error code and an error message.
359363
/// \param[in] _config Custom parser configuration

‎python/test/pyLink_TEST.py

+105-2
Original file line numberDiff line numberDiff line change
@@ -483,12 +483,14 @@ def test_resolveauto_inertialsWithMultipleCollisions(self):
483483
link.inertial().mass_matrix().diagonal_moments())
484484

485485
def test_inertial_values_given_with_auto_set_to_true(self):
486+
# The inertia matrix is specified but should be ignored.
487+
# <mass> is not speicifed so the inertial values should be computed
488+
# based on the collision density value.
486489
sdf = "<?xml version=\"1.0\"?>" + \
487490
"<sdf version=\"1.11\">" + \
488491
" <model name='compound_model'>" + \
489492
" <link name='compound_link'>" + \
490493
" <inertial auto='True'>" + \
491-
" <mass>4.0</mass>" + \
492494
" <pose>1 1 1 2 2 2</pose>" + \
493495
" <inertia>" + \
494496
" <ixx>1</ixx>" + \
@@ -519,11 +521,112 @@ def test_inertial_values_given_with_auto_set_to_true(self):
519521
root.resolve_auto_inertials(errors, sdfParserConfig)
520522
self.assertEqual(len(errors), 0)
521523

522-
self.assertNotEqual(4.0, link.inertial().mass_matrix().mass())
524+
self.assertEqual(2.0, link.inertial().mass_matrix().mass())
523525
self.assertEqual(Pose3d.ZERO, link.inertial().pose())
524526
self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333),
525527
link.inertial().mass_matrix().diagonal_moments())
526528

529+
def test_resolveauto_inertialsWithMass(self):
530+
# The inertia matrix is specified but should be ignored.
531+
# <mass> is speicifed - the auto computed inertial values should
532+
# be scaled based on the desired mass.
533+
sdf = "<?xml version=\"1.0\"?>" + \
534+
"<sdf version=\"1.11\">" + \
535+
" <model name='compound_model'>" + \
536+
" <link name='compound_link'>" + \
537+
" <inertial auto='True'>" + \
538+
" <mass>4.0</mass>" + \
539+
" <pose>1 1 1 2 2 2</pose>" + \
540+
" <inertia>" + \
541+
" <ixx>1</ixx>" + \
542+
" <iyy>1</iyy>" + \
543+
" <izz>1</izz>" + \
544+
" </inertia>" + \
545+
" </inertial>" + \
546+
" <collision name='box_collision'>" + \
547+
" <density>2.0</density>" + \
548+
" <geometry>" + \
549+
" <box>" + \
550+
" <size>1 1 1</size>" + \
551+
" </box>" + \
552+
" </geometry>" + \
553+
" </collision>" + \
554+
" </link>" + \
555+
" </model>" + \
556+
" </sdf>"
557+
558+
root = Root()
559+
sdfParserConfig = ParserConfig()
560+
errors = root.load_sdf_string(sdf, sdfParserConfig)
561+
self.assertEqual(errors, None)
562+
563+
model = root.model()
564+
link = model.link_by_index(0)
565+
errors = []
566+
root.resolve_auto_inertials(errors, sdfParserConfig)
567+
self.assertEqual(len(errors), 0)
568+
569+
self.assertEqual(4.0, link.inertial().mass_matrix().mass())
570+
self.assertEqual(Pose3d.ZERO, link.inertial().pose())
571+
self.assertEqual(Vector3d(0.66666, 0.66666, 0.66666),
572+
link.inertial().mass_matrix().diagonal_moments())
573+
574+
def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
575+
# The inertia matrix is specified but should be ignored.
576+
# <mass> is speicifed - the auto computed inertial values should
577+
# be scaled based on the desired mass.
578+
sdf = "<?xml version=\"1.0\"?>" + \
579+
"<sdf version=\"1.11\">" + \
580+
" <model name='compound_model'>" + \
581+
" <link name='compound_link'>" + \
582+
" <inertial auto='True'>" + \
583+
" <mass>4.0</mass>" + \
584+
" <pose>1 1 1 2 2 2</pose>" + \
585+
" <inertia>" + \
586+
" <ixx>1</ixx>" + \
587+
" <iyy>1</iyy>" + \
588+
" <izz>1</izz>" + \
589+
" </inertia>" + \
590+
" </inertial>" + \
591+
" <collision name='box_collision'>" + \
592+
" <pose>0.0 0.0 0.5 0 0 0</pose>" + \
593+
" <density>2.0</density>" + \
594+
" <geometry>" + \
595+
" <box>" + \
596+
" <size>1 1 1</size>" + \
597+
" </box>" + \
598+
" </geometry>" + \
599+
" </collision>" + \
600+
" <collision name='box_collision2'>" + \
601+
" <pose>0.0 0.0 -0.5 0 0 0</pose>" + \
602+
" <density>4.0</density>" + \
603+
" <geometry>" + \
604+
" <box>" + \
605+
" <size>1 1 1</size>" + \
606+
" </box>" + \
607+
" </geometry>" + \
608+
" </collision>" + \
609+
" </link>" + \
610+
" </model>" + \
611+
"</sdf>"
612+
613+
root = Root()
614+
sdfParserConfig = ParserConfig()
615+
errors = root.load_sdf_string(sdf, sdfParserConfig)
616+
self.assertEqual(errors, None)
617+
618+
model = root.model()
619+
link = model.link_by_index(0)
620+
errors = []
621+
root.resolve_auto_inertials(errors, sdfParserConfig)
622+
self.assertEqual(len(errors), 0)
623+
624+
self.assertEqual(4.0, link.inertial().mass_matrix().mass())
625+
self.assertEqual(Pose3d(0, 0, -0.166667, 0, 0, 0),
626+
link.inertial().pose())
627+
self.assertEqual(Vector3d(1.55556, 1.55556, 0.666667),
628+
link.inertial().mass_matrix().diagonal_moments())
629+
527630
def test_resolveauto_inertialsCalledWithAutoFalse(self):
528631
sdf = "<?xml version=\"1.0\"?>" + \
529632
" <sdf version=\"1.11\">" + \

‎src/Collision.cc

+1-11
Original file line numberDiff line numberDiff line change
@@ -291,18 +291,8 @@ void Collision::CalculateInertial(
291291
// 3. DensityDefault value.
292292
else
293293
{
294-
// If density was not explicitly set, send a warning
295-
// about the default value being used
294+
// If density was not explicitly set, default value is used
296295
density = DensityDefault();
297-
Error densityMissingErr(
298-
ErrorCode::ELEMENT_MISSING,
299-
"Collision is missing a <density> child element. "
300-
"Using a default density value of " +
301-
std::to_string(DensityDefault()) + " kg/m^3. "
302-
);
303-
enforceConfigurablePolicyCondition(
304-
_config.WarningsPolicy(), densityMissingErr, _errors
305-
);
306296
}
307297

308298
// If this Collision's auto inertia params have not been set, then use the

‎src/Link.cc

+21-3
Original file line numberDiff line numberDiff line change
@@ -211,7 +211,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
211211
// If auto is to true but user has still provided
212212
// inertial values
213213
if (inertialElem->HasElement("pose") ||
214-
inertialElem->HasElement("mass") ||
215214
inertialElem->HasElement("inertia"))
216215
{
217216
Error err(
@@ -681,7 +680,27 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
681680
totalInertia = totalInertia + collisionInertia;
682681
}
683682

684-
this->dataPtr->inertial = totalInertia;
683+
// If mass is specified, scale inertia to match desired mass
684+
auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
685+
if (inertialElem->HasElement("mass"))
686+
{
687+
double mass = inertialElem->Get<double>("mass");
688+
const gz::math::MassMatrix3d &totalMassMatrix = totalInertia.MassMatrix();
689+
// normalize to get the unit mass matrix
690+
gz::math::MassMatrix3d unitMassMatrix(1.0,
691+
totalMassMatrix.DiagonalMoments() / totalMassMatrix.Mass(),
692+
totalMassMatrix.OffDiagonalMoments() / totalMassMatrix.Mass());
693+
// scale the final inertia to match specified mass
694+
this->dataPtr->inertial = gz::math::Inertiald(
695+
gz::math::MassMatrix3d(mass,
696+
mass * unitMassMatrix.DiagonalMoments(),
697+
mass * unitMassMatrix.OffDiagonalMoments()),
698+
totalInertia.Pose());
699+
}
700+
else
701+
{
702+
this->dataPtr->inertial = totalInertia;
703+
}
685704

686705
// If CalculateInertial() was called with SAVE_CALCULATION
687706
// configuration then set autoInertiaSaved to true
@@ -695,7 +714,6 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
695714
{
696715
this->dataPtr->autoInertiaSaved = true;
697716
// Write calculated inertia values to //link/inertial element
698-
auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
699717
inertialElem->GetElement("pose")->GetValue()->Set<gz::math::Pose3d>(
700718
totalInertia.Pose());
701719
inertialElem->GetElement("mass")->GetValue()->Set<double>(

‎src/Link_TEST.cc

+162-2
Original file line numberDiff line numberDiff line change
@@ -727,12 +727,15 @@ TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions)
727727
/////////////////////////////////////////////////
728728
TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)
729729
{
730+
// A model with link inertial auto set to true.
731+
// The inertia matrix is specified but should be ignored.
732+
// <mass> is not speicifed so the inertial values should be computed
733+
// based on the collision density value.
730734
std::string sdf = "<?xml version=\"1.0\"?>"
731735
"<sdf version=\"1.11\">"
732736
" <model name='compound_model'>"
733737
" <link name='compound_link'>"
734738
" <inertial auto='true'>"
735-
" <mass>4.0</mass>"
736739
" <pose>1 1 1 2 2 2</pose>"
737740
" <inertia>"
738741
" <ixx>1</ixx>"
@@ -763,12 +766,169 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)
763766
root.ResolveAutoInertials(errors, sdfParserConfig);
764767
EXPECT_TRUE(errors.empty());
765768

766-
EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass());
769+
EXPECT_EQ(2.0, link->Inertial().MassMatrix().Mass());
767770
EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose());
768771
EXPECT_EQ(gz::math::Vector3d(0.33333, 0.33333, 0.33333),
769772
link->Inertial().MassMatrix().DiagonalMoments());
770773
}
771774

775+
/////////////////////////////////////////////////
776+
TEST(DOMLink, ResolveAutoInertialsWithMass)
777+
{
778+
// A model with link inertial auto set to true.
779+
// The inertia matrix is specified but should be ignored.
780+
// <mass> is specified - the auto computed inertial values should
781+
// be scaled based on the desired mass.
782+
std::string sdf = "<?xml version=\"1.0\"?>"
783+
"<sdf version=\"1.11\">"
784+
" <model name='compound_model'>"
785+
" <link name='compound_link'>"
786+
" <inertial auto='true'>"
787+
" <mass>4.0</mass>"
788+
" <pose>1 1 1 2 2 2</pose>"
789+
" <inertia>"
790+
" <ixx>1</ixx>"
791+
" <iyy>1</iyy>"
792+
" <izz>1</izz>"
793+
" </inertia>"
794+
" </inertial>"
795+
" <collision name='box_collision'>"
796+
" <density>2.0</density>"
797+
" <geometry>"
798+
" <box>"
799+
" <size>1 1 1</size>"
800+
" </box>"
801+
" </geometry>"
802+
" </collision>"
803+
" </link>"
804+
" </model>"
805+
" </sdf>";
806+
807+
sdf::Root root;
808+
const sdf::ParserConfig sdfParserConfig;
809+
sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
810+
EXPECT_TRUE(errors.empty());
811+
EXPECT_NE(nullptr, root.Element());
812+
813+
const sdf::Model *model = root.Model();
814+
const sdf::Link *link = model->LinkByIndex(0);
815+
root.ResolveAutoInertials(errors, sdfParserConfig);
816+
EXPECT_TRUE(errors.empty());
817+
818+
EXPECT_DOUBLE_EQ(4.0, link->Inertial().MassMatrix().Mass());
819+
EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose());
820+
EXPECT_EQ(gz::math::Vector3d(0.66666, 0.66666, 0.66666),
821+
link->Inertial().MassMatrix().DiagonalMoments());
822+
}
823+
824+
/////////////////////////////////////////////////
825+
TEST(DOMLink, ResolveAutoInertialsWithMassAndMultipleCollisions)
826+
{
827+
// A model with link inertial auto set to true.
828+
// The inertia matrix is specified but should be ignored.
829+
// <mass> is specified - the auto computed inertial values should
830+
// be scaled based on the desired mass.
831+
// The model contains two collisions with different sizes and densities
832+
// and a lumped center of mass at the link origin.
833+
std::string sdf = "<?xml version=\"1.0\"?>"
834+
"<sdf version=\"1.11\">"
835+
" <model name='compound_model'>"
836+
" <link name='compound_link'>"
837+
" <inertial auto='true'>"
838+
" <mass>12.0</mass>"
839+
" <pose>1 1 1 2 2 2</pose>"
840+
" <inertia>"
841+
" <ixx>1</ixx>"
842+
" <iyy>1</iyy>"
843+
" <izz>1</izz>"
844+
" </inertia>"
845+
" </inertial>"
846+
" <collision name='cube_collision'>"
847+
" <pose>0.0 0.0 0.5 0 0 0</pose>"
848+
" <density>4.0</density>"
849+
" <geometry>"
850+
" <box>"
851+
" <size>1 1 1</size>"
852+
" </box>"
853+
" </geometry>"
854+
" </collision>"
855+
" <collision name='box_collision'>"
856+
" <pose>0.0 0.0 -1.0 0 0 0</pose>"
857+
" <density>1.0</density>"
858+
" <geometry>"
859+
" <box>"
860+
" <size>1 1 2</size>"
861+
" </box>"
862+
" </geometry>"
863+
" </collision>"
864+
" </link>"
865+
" </model>"
866+
"</sdf>";
867+
868+
sdf::Root root;
869+
const sdf::ParserConfig sdfParserConfig;
870+
sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
871+
EXPECT_TRUE(errors.empty());
872+
EXPECT_NE(nullptr, root.Element());
873+
874+
const sdf::Model *model = root.Model();
875+
const sdf::Link *link = model->LinkByIndex(0);
876+
root.ResolveAutoInertials(errors, sdfParserConfig);
877+
EXPECT_TRUE(errors.empty());
878+
879+
// Expect mass value from //inertial/mass
880+
EXPECT_DOUBLE_EQ(12.0, link->Inertial().MassMatrix().Mass());
881+
882+
// Inertial values based on density before scaling to match specified mass
883+
//
884+
// Collision geometries:
885+
//
886+
// --------- z = 1
887+
// | |
888+
// | c | cube on top, side length 1.0, density 4.0
889+
// | |
890+
// |-------| z = 0
891+
// | |
892+
// | |
893+
// | |
894+
// | c | box on bottom, size 1x1x2, density 1.0
895+
// | |
896+
// | |
897+
// | |
898+
// --------- z = -2
899+
//
900+
// Top cube: volume = 1.0, mass = 4.0, center of mass z = 0.5
901+
// Bottom box: volume = 2.0, mass = 2.0, center of mass z = -1.0
902+
//
903+
// Total mass from density: 6.0
904+
// Lumped center of mass z = (4.0 * 0.5 + 2.0 * (-1.0)) / 6.0 = 0.0
905+
EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose());
906+
907+
// Moment of inertia at center of each shape
908+
// Top cube: Ixx = Iyy = Izz = 4.0 / 12 * (1*1 + 1*1) = 8/12 = 2/3
909+
// Bottom box: Ixx = Iyy = 2.0 / 12 * (1*1 + 2*2) = 10/12 = 5/6
910+
// Izz = 2.0 / 12 * (1*1 + 1*1) = 4/12 = 1/3
911+
//
912+
// Lumped moment of inertia at lumped center of mass
913+
// Ixx = sum(Ixx_i + mass_i * center_of_mass_z_i^2) for i in [top, bottom]
914+
// Iyy = Ixx
915+
// Izz = Izz_top + Izz_bottom
916+
//
917+
// Ixx = Iyy = (2/3 + 4.0 * 0.5 * 0.5) + (5/6 + 2.0 * (-1.0) * (-1.0))
918+
// = (2/3 + 1) + (5/6 + 2.0)
919+
// = 5/3 + 17/6
920+
// = 27/6 = 9/2 = 4.5
921+
//
922+
// Izz = 2/3 + 1/3 = 1.0
923+
924+
// Then scale the inertias according to the specified mass of 12.0
925+
// mass_ratio = 12.0 / 6.0 = 2.0
926+
// Ixx = Iyy = mass_ratio * Ixx_unscaled = 2.0 * 4.5 = 9.0
927+
// Izz = mass_ratio * Izz_unscaled = 2.0 * 1.0 = 2.0
928+
EXPECT_EQ(gz::math::Vector3d(9.0, 9.0, 2.0),
929+
link->Inertial().MassMatrix().DiagonalMoments());
930+
}
931+
772932
/////////////////////////////////////////////////
773933
TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse)
774934
{

0 commit comments

Comments
 (0)