From 6ffe66975f5b5260929eecb2f480d4f1d4a5638f Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 22 Mar 2023 02:39:40 +0000 Subject: [PATCH] URDF->SDF handle links with no inertia or small mass (#1238) * Adding tests that catch warnings when urdf have no inertia element, or if mass is too small Signed-off-by: Aaron Chong * Promote link inertia and mass related urdf2sdf sdfdbg to sdfwarn with more verbose messages Signed-off-by: Aaron Chong * Added flag for converting urdf links with small or no mass to frames Signed-off-by: Aaron Chong * Converts urdf links with small or no mass to frames, added tests Signed-off-by: Aaron Chong * Adding warning when conversion happens, added tests for small masses, being explicit about epsilon in equal Signed-off-by: Aaron Chong * Fix cpplint errors Signed-off-by: Aaron Chong * Adding integration test Signed-off-by: Aaron Chong * Fix lint Signed-off-by: Aaron Chong * Use camelCase Signed-off-by: Aaron Chong * Added URDFMinimumAllowedLinkMass Signed-off-by: Aaron Chong * Fix tests expecting warnings when converting to frames Signed-off-by: Aaron Chong * Adding inline contains and notContains to test_utils Signed-off-by: Aaron Chong * Using RedirectConsoleStream and ScopeExit Signed-off-by: Aaron Chong * Refactor to use Root::LoadSdfString Signed-off-by: Aaron Chong * Removing debug message when converted frame is from a root link Signed-off-by: Aaron Chong * Added attached_to for frames during conversion, using < instead of math::equals Signed-off-by: Aaron Chong * Update brief of URDFSetConvertLinkWithNoMassToFrame to mention case of no inertial frame Signed-off-by: Aaron Chong * Remove stale commentted code Signed-off-by: Aaron Chong * Update comment about the errors we are expecting Signed-off-by: Aaron Chong * Convert to frame by default, remove minimal mass option, refactor implementation to handle lumping, modified unit tests Signed-off-by: Aaron Chong * Only convert to frame when parent joint is fixed, attaches and transforms pose to parent link, leaves out the fixed joint Signed-off-by: Aaron Chong * Rephrased conversion error, modified unit tests Signed-off-by: Aaron Chong * prints zero mass errors as well when conversion to frame fails Signed-off-by: Aaron Chong * integration tests that mimic the unit tests Signed-off-by: Aaron Chong * Added integration test with valid and invalid use of force torque sensor where massless child links occur Signed-off-by: Aaron Chong * Fix lint Signed-off-by: Aaron Chong * Fix joint reduction logic, more specific error messages, more targeted unit tests for each case Signed-off-by: Aaron Chong * Convert joints to frames when converting links, attach them to converted links Signed-off-by: Aaron Chong * Integration tests with child fixed links as well Signed-off-by: Aaron Chong * Change sdferr to sdfwarn, no way to use ParserConfig::WarningsPolicy for now Signed-off-by: Aaron Chong * sdferr to sdfwarn for the case where conversion to frame is attempted Signed-off-by: Aaron Chong * Removing case within converting to frame, where parent joint turns into revolute joint, that is not covered Signed-off-by: Aaron Chong * Reduced scope of implementation, more specific warning messages Signed-off-by: Aaron Chong * Remove mention of parser config in warning, since that is not typically used by workflows Signed-off-by: Aaron Chong * Integration tests revisited and modified, removed printouts Signed-off-by: Aaron Chong * Test case showing ParserConfig::URDFSetPreserveFixedJoint(true) overrides gazebo tags for joint lumping, warnings with more information and placeholder url Signed-off-by: Aaron Chong * Use sdf::testing::contains instead of local contains functions in testing (#1251) Signed-off-by: Aaron Chong * Remove unused InitSDF, added TODO for warning when joints are converted/dropped Signed-off-by: Aaron Chong * Cleaned up if else cases Signed-off-by: Aaron Chong * Using << operator of Errors Signed-off-by: Aaron Chong * Replacing placeholder url with expected URL for the documentation from sdf_tutorials#88 Signed-off-by: Aaron Chong * URL to tutorial as a constant, removing checking URL from test, just in case links change, we are not testing for that anyway Signed-off-by: Aaron Chong * Refactored fixed child joint logic as it is never reached Signed-off-by: Aaron Chong * Reduce number of if statements, renaming to only check if parent joint is reduced Signed-off-by: Aaron Chong --------- Signed-off-by: Aaron Chong --- src/ign_TEST.cc | 131 +- src/parser_TEST.cc | 75 +- src/parser_urdf.cc | 135 +- src/parser_urdf_TEST.cc | 1466 +++++++++++++++++ test/integration/CMakeLists.txt | 1 + test/integration/deprecated_specs.cc | 11 +- ...orce_torque_sensor_lumped_and_reduced.urdf | 46 + ...rce_torque_sensor_massless_child_link.urdf | 49 + test/integration/pose_1_9_sdf.cc | 56 +- test/integration/urdf_to_sdf.cc | 1047 ++++++++++++ test/test_utils.hh | 12 + 11 files changed, 2859 insertions(+), 170 deletions(-) create mode 100644 test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf create mode 100644 test/integration/invalid_force_torque_sensor_massless_child_link.urdf create mode 100644 test/integration/urdf_to_sdf.cc diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 9a62eb1e2..875a0b932 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -28,6 +28,7 @@ #include "sdf/SDFImpl.hh" #include "sdf/sdf_config.h" #include "test_config.h" +#include "test_utils.hh" #ifdef _WIN32 #define popen _popen @@ -950,12 +951,6 @@ TEST(print, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } } -////////////////////////////////////////////////// -static bool contains(const std::string &_a, const std::string &_b) -{ - return _a.find(_b) != std::string::npos; -} - ///////////////////////////////////////////////// TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { @@ -966,14 +961,14 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "1 2 3 30.009 44.991 -60.009"); // Printing with in_degrees output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -982,7 +977,7 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -991,7 +986,7 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1000,7 +995,7 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1009,7 +1004,7 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1018,7 +1013,7 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1033,14 +1028,14 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "1 2 3 0.523756 0.785241 -1.04735"); // Printing with in_degrees output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.0087"); @@ -1049,7 +1044,7 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1058,7 +1053,7 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1067,7 +1062,7 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1076,7 +1071,7 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.0087"); @@ -1085,7 +1080,7 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1100,7 +1095,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 0.391948 0.200425 -0.532046 0.723279"); @@ -1108,7 +1103,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1117,7 +1112,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1126,7 +1121,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1135,7 +1130,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1144,7 +1139,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1153,7 +1148,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1171,14 +1166,14 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "1 2 3 30.009 44.991 -60.009"); // Printing with in_degrees output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1187,7 +1182,7 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1196,7 +1191,7 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1205,7 +1200,7 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1214,7 +1209,7 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1223,7 +1218,7 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1241,14 +1236,14 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "1 2 3 0.523756 0.785241 -1.04735"); // Printing with in_degrees output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.0087"); @@ -1257,7 +1252,7 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1266,7 +1261,7 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1275,7 +1270,7 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1284,7 +1279,7 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.0087"); @@ -1293,7 +1288,7 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1312,7 +1307,7 @@ TEST(print_includes_rotations_in_quaternions, std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 0.391948 0.200425 -0.532046 0.723279"); @@ -1320,7 +1315,7 @@ TEST(print_includes_rotations_in_quaternions, output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1329,7 +1324,7 @@ TEST(print_includes_rotations_in_quaternions, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1338,7 +1333,7 @@ TEST(print_includes_rotations_in_quaternions, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1347,7 +1342,7 @@ TEST(print_includes_rotations_in_quaternions, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1356,7 +1351,7 @@ TEST(print_includes_rotations_in_quaternions, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1365,7 +1360,7 @@ TEST(print_includes_rotations_in_quaternions, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1384,14 +1379,14 @@ TEST(print_rotations_in_unnormalized_degrees, std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "1 2 3 30.009 44.991 -60.009"); // Printing with in_degrees output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1400,7 +1395,7 @@ TEST(print_rotations_in_unnormalized_degrees, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1409,7 +1404,7 @@ TEST(print_rotations_in_unnormalized_degrees, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.991 -60"); @@ -1418,7 +1413,7 @@ TEST(print_rotations_in_unnormalized_degrees, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60"); @@ -1427,7 +1422,7 @@ TEST(print_rotations_in_unnormalized_degrees, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.991 -60.009"); @@ -1436,7 +1431,7 @@ TEST(print_rotations_in_unnormalized_degrees, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1452,14 +1447,14 @@ TEST(print_rotations_in_unnormalized_radians, std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "1 2 3 -5.75943 -11.78112 5.23583"); // Printing with in_degrees output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.9915 -60.009"); @@ -1468,7 +1463,7 @@ TEST(print_rotations_in_unnormalized_radians, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1477,7 +1472,7 @@ TEST(print_rotations_in_unnormalized_radians, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 44.9915 -60"); @@ -1486,7 +1481,7 @@ TEST(print_rotations_in_unnormalized_radians, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.9915 -60"); @@ -1495,7 +1490,7 @@ TEST(print_rotations_in_unnormalized_radians, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.9915 -60.009"); @@ -1504,7 +1499,7 @@ TEST(print_rotations_in_unnormalized_radians, IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + "--snap-tolerance 0.01 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1519,7 +1514,7 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str( IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.9915 -60.009"); @@ -1527,7 +1522,7 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) output = custom_exec_str( IgnCommand() + " sdf --degrees -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30.009 44.9915 -60.009"); @@ -1536,7 +1531,7 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1544,7 +1539,7 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) output = custom_exec_str( IgnCommand() + " sdf -p --snap-to-degrees 5 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); @@ -1552,7 +1547,7 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) output = custom_exec_str( IgnCommand() + " sdf --snap-to-degrees 5 -p " + path + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 45 -60"); } @@ -1570,7 +1565,7 @@ TEST(print_snap_to_degrees_tolerance_too_high, " --snap-to-degrees 5 " + " --snap-tolerance 4" + SdfVersion()); ASSERT_FALSE(output.empty()); - EXPECT_PRED2(contains, output, + EXPECT_PRED2(sdf::testing::contains, output, "" "1 2 3 30 50 60"); } diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 012d15db5..8f8664e08 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -24,6 +24,7 @@ #include "sdf/Console.hh" #include "sdf/Filesystem.hh" #include "test_config.h" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(Parser, initStringTrim) @@ -277,13 +278,6 @@ TEST(Parser, NameUniqueness) } } -///////////////////////////////////////////////// -/// Check that _a contains _b -static bool contains(const std::string &_a, const std::string &_b) -{ - return _a.find(_b) != std::string::npos; -} - ///////////////////////////////////////////////// TEST(Parser, SyntaxErrorInValues) { @@ -301,10 +295,11 @@ TEST(Parser, SyntaxErrorInValues) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Unable to set value [bad 0 0 0 0 0] for key [pose]"); - EXPECT_PRED2(contains, buffer.str(), "bad_syntax_pose.sdf:L5"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "bad_syntax_pose.sdf:L5"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/pose:"); } { @@ -315,10 +310,11 @@ TEST(Parser, SyntaxErrorInValues) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Unable to set value [bad ] for key[linear]"); - EXPECT_PRED2(contains, buffer.str(), "bad_syntax_double.sdf:L7"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "bad_syntax_double.sdf:L7"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/" "link[@name=\"link\"]/velocity_decay/linear:"); } @@ -330,10 +326,11 @@ TEST(Parser, SyntaxErrorInValues) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Unable to set value [0 1 bad ] for key[gravity]"); - EXPECT_PRED2(contains, buffer.str(), "bad_syntax_vector.sdf:L4"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "bad_syntax_vector.sdf:L4"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/gravity:"); } @@ -363,15 +360,15 @@ TEST(Parser, MissingRequiredAttributesErrors) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string( static_cast(sdf::ErrorCode::ATTRIBUTE_MISSING))); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Required attribute[name] in element[link] is not specified " "in SDF."); - EXPECT_PRED2(contains, buffer.str(), "box_bad_test.world:L6"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "box_bad_test.world:L6"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/model[@name=\"box\"]/link:"); } @@ -401,14 +398,15 @@ TEST(Parser, IncludesErrors) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string( static_cast(sdf::ErrorCode::ATTRIBUTE_MISSING))); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), " element missing 'uri' attribute"); - EXPECT_PRED2(contains, buffer.str(), "includes_missing_uri.sdf:L5"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "includes_missing_uri.sdf:L5"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/include[0]:"); } { @@ -421,14 +419,15 @@ TEST(Parser, IncludesErrors) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string( static_cast(sdf::ErrorCode::URI_LOOKUP))); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Unable to find uri[missing_model]"); - EXPECT_PRED2(contains, buffer.str(), "includes_missing_model.sdf:L6"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "includes_missing_model.sdf:L6"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/include[0]/uri:"); } { @@ -446,15 +445,16 @@ TEST(Parser, IncludesErrors) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string(static_cast(sdf::ErrorCode::URI_LOOKUP))); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Unable to resolve uri[box_missing_config]"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "since it does not contain a model.config"); - EXPECT_PRED2(contains, buffer.str(), "includes_model_without_sdf.sdf:L6"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "includes_model_without_sdf.sdf:L6"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/include[0]/uri:"); } { @@ -472,15 +472,16 @@ TEST(Parser, IncludesErrors) sdf::init(sdf); sdf::readFile(path, sdf); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string( static_cast(sdf::ErrorCode::ELEMENT_MISSING))); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Failed to find top level / / for " "\n"); - EXPECT_PRED2(contains, buffer.str(), "includes_without_top_level.sdf:L6"); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "includes_without_top_level.sdf:L6"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "/sdf/world[@name=\"default\"]/include[0]/uri:"); } diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 945062049..b97fabc93 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -31,7 +31,9 @@ #include #include +#include "sdf/Error.hh" #include "sdf/sdf.hh" +#include "sdf/Types.hh" #include "XmlUtils.hh" #include "SDFExtension.hh" @@ -61,7 +63,8 @@ bool g_initialRobotPoseValid = false; std::set g_fixedJointsTransformedInRevoluteJoints; std::set g_fixedJointsTransformedInFixedJoints; const int g_outputDecimalPrecision = 16; - +const char kSdformatUrdfExtensionUrl[] = + "http://sdformat.org/tutorials?tut=sdformat_urdf_extensions"; /// \brief parser xml string into urdf::Vector3 /// \param[in] _key XML key where vector3 value might be @@ -2663,48 +2666,126 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) void CreateSDF(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link) { - // must have an block and cannot have zero mass. - // allow det(I) == zero, in the case of point mass geoms. + // Links without an block will be considered to have zero mass. + const bool linkHasZeroMass = !_link->inertial || _link->inertial->mass <= 0; + + // link must have an block and cannot have zero mass, if its parent + // joint and none of its child joints are reduced. + // allow det(I) == zero, in the case of point mass geoms. // @todo: keyword "world" should be a constant defined somewhere else - if (_link->name != "world" && - ((!_link->inertial) || gz::math::equal(_link->inertial->mass, 0.0))) + if (_link->name != "world" && linkHasZeroMass) { - if (!_link->child_links.empty()) + Errors nonJointReductionErrors; + std::stringstream errorStream; + errorStream << "urdf2sdf: link[" << _link->name << "] has "; + if (!_link->inertial) { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" - << static_cast(_link->child_links.size()) - << "] children links ignored.\n"; + errorStream << "no block defined. "; } - - if (!_link->child_joints.empty()) + else { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" - << static_cast(_link->child_links.size()) - << "] children joints ignored.\n"; + errorStream << "a mass value of less than or equal to zero. "; } + errorStream << "Please ensure this link has a valid mass to prevent any " + << "missing links or joints in the resulting SDF model."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + + // if the parent joint is reduced, which resolves the massless issue of this + // link, no warnings will be emitted + bool parentJointReduced = _link->parent_joint && + FixedJointShouldBeReduced(_link->parent_joint) && + g_reduceFixedJoints; - if (_link->parent_joint) + if (!parentJointReduced) { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, " - << "parent joint [" << _link->parent_joint->name - << "] ignored.\n"; - } + // check if joint lumping was turned off for the parent joint + if (_link->parent_joint) + { + if (_link->parent_joint->type == urdf::Joint::FIXED) + { + errorStream << "urdf2sdf: allowing joint lumping by removing any " + << " or " + << "gazebo tag on fixed parent joint[" + << _link->parent_joint->name + << "], as well as ensuring that " + << "ParserConfig::URDFPreserveFixedJoint is false, could " + << "help resolve this warning. See " + << std::string(kSdformatUrdfExtensionUrl) + << " for more information about this behavior."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, not modeled in sdf\n"; - return; + errorStream << "urdf2sdf: parent joint[" + << _link->parent_joint->name + << "] ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + + // check if joint lumping was turned off for child joints + if (!_link->child_joints.empty()) + { + for (const auto &cj : _link->child_joints) + { + // Lumping child joints occur before CreateSDF, so if it does occur + // this link would already contain the lumped mass from the child + // link. If a child joint is still fixed at this point, it means joint + // lumping has been disabled. + if (cj->type == urdf::Joint::FIXED) + { + errorStream << "urdf2sdf: allowing joint lumping by removing any " + << " or " + << "gazebo tag on fixed child joint[" + << cj->name + << "], as well as ensuring that " + << "ParserConfig::URDFPreserveFixedJoint is false, " + << "could help resolve this warning. See " + << std::string(kSdformatUrdfExtensionUrl) + << " for more information about this behavior."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + } + + errorStream << "urdf2sdf: [" + << static_cast(_link->child_joints.size()) + << "] child joints ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + errorStream << "urdf2sdf: [" + << static_cast(_link->child_links.size()) + << "] child links ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + + // Emit all accumulated warnings and return + sdfwarn << nonJointReductionErrors; + errorStream << "urdf2sdf: link[" << _link->name + << "] is not modeled in sdf."; + sdfwarn << Error(ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + return; + } } - // create block for non fixed joint attached bodies + // create block for non fixed joint attached bodies that have mass if ((_link->getParent() && _link->getParent()->name == "world") || !g_reduceFixedJoints || (!_link->parent_joint || !FixedJointShouldBeReduced(_link->parent_joint))) { - CreateLink(_root, _link, gz::math::Pose3d::Zero); + if (!linkHasZeroMass) + { + CreateLink(_root, _link, gz::math::Pose3d::Zero); + } } // recurse into children diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 70b3ef8c5..97be40a89 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -21,6 +21,7 @@ #include "sdf/sdf.hh" #include "parser_urdf.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// std::string getMinimalUrdfTxt() @@ -698,6 +699,108 @@ TEST(URDFParser, ASSERT_EQ(jointType, "fixed"); } +///////////////////////////////////////////////// +TEST(URDFParser, ParserConfigSetPreserveFixedJointTrueOverridesGazeboTags) +{ + { + std::ostringstream fixedJointShouldBeLumped; + fixedJointShouldBeLumped << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " false" + << " " + << ""; + + // Set the config option to preserve fixed joints, this should override the + // gazebo tag false + // and disable lumping. + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + + // Check that there is a fixed joint in the converted SDF + sdf::SDF fixedJointShouldBeLumpedSDF; + convertUrdfStrToSdf(fixedJointShouldBeLumped.str(), + fixedJointShouldBeLumpedSDF, config); + sdf::ElementPtr elem = fixedJointShouldBeLumpedSDF.Root(); + ASSERT_NE(nullptr, elem); + ASSERT_TRUE(elem->HasElement("model")); + elem = elem->GetElement("model"); + ASSERT_TRUE(elem->HasElement("joint")); + elem = elem->GetElement("joint"); + std::string jointType = elem->Get("type"); + ASSERT_EQ(jointType, "fixed"); + } + + { + std::ostringstream fixedJointPreserveFixedJoint; + fixedJointPreserveFixedJoint << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " false" + << " " + << ""; + + // Set the config option to preserve fixed joints, this should override the + // gazebo tag false and disable + // lumping. + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + + // Check that there is a fixed joint in the converted SDF + sdf::SDF fixedJointPreserveFixedJointSDF; + convertUrdfStrToSdf(fixedJointPreserveFixedJoint.str(), + fixedJointPreserveFixedJointSDF, config); + sdf::ElementPtr elem = fixedJointPreserveFixedJointSDF.Root(); + ASSERT_NE(nullptr, elem); + ASSERT_TRUE(elem->HasElement("model")); + elem = elem->GetElement("model"); + ASSERT_TRUE(elem->HasElement("joint")); + elem = elem->GetElement("joint"); + std::string jointType = elem->Get("type"); + ASSERT_EQ(jointType, "fixed"); + } +} + ///////////////////////////////////////////////// TEST(URDFParser, CheckFixedJointOptions_NoOption_Repeated) { @@ -994,6 +1097,1369 @@ TEST(URDFParser, ParseWhitespace) EXPECT_EQ("1000", std::string(mu2Elem->GetText())); } +///////////////////////////////////////////////// +TEST(URDFParser, LinksWithMassIssuesIdentified) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // explicitly zero mass + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has a mass value of less than or equal to zero. Please " + "ensure this link has a valid mass to prevent any missing links or " + "joints in the resulting SDF model"); + } + + // no block defined + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + } + + // negative mass + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has a mass value of less than or equal to zero. Please " + "ensure this link has a valid mass to prevent any missing links or " + "joints in the resulting SDF model"); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithNoFixedJoints) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithFixedParentJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // joint lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 visual lumps into link1 + tinyxml2::XMLElement *lumpedVisual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, lumpedVisual); + ASSERT_NE(nullptr, lumpedVisual->Attribute("name")); + EXPECT_EQ("link1_fixed_joint_lump__link2_visual", + std::string(lumpedVisual->Attribute("name"))); + + // link2 collision lumps into link1 + tinyxml2::XMLElement *lumpedCollision = + link->FirstChildElement("collision"); + ASSERT_NE(nullptr, lumpedCollision); + ASSERT_NE(nullptr, lumpedCollision->Attribute("name")); + EXPECT_EQ("link1_fixed_joint_lump__link2_collision", + std::string(lumpedCollision->Attribute("name"))); + + // joint1_2 converted into frame, attached to link1 + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link1", std::string(frame->Attribute("attached_to"))); + + // link2 converted into frame, attached to joint1_2 + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link2", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint1_2", std::string(frame->Attribute("attached_to"))); + + // joint2_3 will change to be relative to link1 + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(joint->Attribute("name"))); + tinyxml2::XMLElement *jointPose = joint->FirstChildElement("pose"); + ASSERT_NE(nullptr, jointPose); + ASSERT_NE(nullptr, jointPose->Attribute("relative_to")); + EXPECT_EQ("link1", std::string(jointPose->Attribute("relative_to"))); + + // link3 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link3", std::string(link->Attribute("name"))); + tinyxml2::XMLElement *linkPose = link->FirstChildElement("pose"); + ASSERT_NE(nullptr, linkPose); + ASSERT_NE(nullptr, linkPose->Attribute("relative_to")); + EXPECT_EQ("joint2_3", std::string(linkPose->Attribute("relative_to"))); + } + + // Disabling lumping using gazebo tags, fails with warnings suggesting to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } + + // Disabling lumping using ParserConfig::URDFPreserveFixedJoint, fails with + // warnings suggesting to set to false + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + parser.InitModelString(str, config, &sdfResult); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithFixedChildJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs, mass of link3 lumped into link2 + // link3 and joint2_3 converted into frames + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // mass of link3 lumped into link2, link2 not converted to frame + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // link2 visual and collision remain + tinyxml2::XMLElement *visual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, visual); + ASSERT_NE(nullptr, visual->Attribute("name")); + EXPECT_EQ("link2_visual", std::string(visual->Attribute("name"))); + tinyxml2::XMLElement *collision = link->FirstChildElement("collision"); + ASSERT_NE(nullptr, collision); + ASSERT_NE(nullptr, collision->Attribute("name")); + EXPECT_EQ("link2_collision", std::string(collision->Attribute("name"))); + + // joint1_2 + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // joint2_3 converted into a frame + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link2", std::string(frame->Attribute("attached_to"))); + + // link3 converted into a frame + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("attached_to"))); + } + + // turn off lumping with gazebo tag, conversion fails with warnings to suggest + // removing gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } + + // Disabling lumping using ParserConfig::URDFPreserveFixedJoint, fails with + // warnings suggesting to set to false + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + parser.InitModelString(str, config, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithAllFixedJointsButLumpingOff) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // turn off all lumping with gazebo tags, conversion fails with suggestions to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + true + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } + + // Disabling lumping using ParserConfig::URDFPreserveFixedJoint, fails with + // warnings suggesting to set to false + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + parser.InitModelString(str, config, &sdfResult); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassLeafLink) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision lumped into link2 + tinyxml2::XMLElement *visual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, visual); + ASSERT_NE(nullptr, visual->Attribute("name")); + EXPECT_EQ("link2_fixed_joint_lump__link3_visual", + std::string(visual->Attribute("name"))); + tinyxml2::XMLElement *collision = link->FirstChildElement("collision"); + ASSERT_NE(nullptr, collision); + ASSERT_NE(nullptr, collision->Attribute("name")); + EXPECT_EQ("link2_fixed_joint_lump__link3_collision", + std::string(collision->Attribute("name"))); + + // joint2_3 converted into a frame + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link2", std::string(frame->Attribute("attached_to"))); + + // link3 converted into a frame + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("attached_to"))); + } + + // lumping and reduction turned off with gazebo tag, expect link3 and joint2_3 + // to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig defaultConfig; + parser.InitModelString(str, defaultConfig, &sdfResult); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision not lumped into link2 + tinyxml2::XMLElement *visual = link->FirstChildElement("visual"); + EXPECT_EQ(nullptr, visual); + tinyxml2::XMLElement *collision = link->FirstChildElement("collision"); + EXPECT_EQ(nullptr, collision); + + // joint2_3 ignored + joint = joint->NextSiblingElement("joint"); + EXPECT_EQ(nullptr, joint); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + } + + // ParserConfig::URDFSetPreserveFixedJoint set to true, expect link3 and + // joint2_3 to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfResult; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + parser.InitModelString(str, config, &sdfResult); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + tinyxml2::XMLElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + tinyxml2::XMLElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + tinyxml2::XMLElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision not lumped into link2 + tinyxml2::XMLElement *visual = link->FirstChildElement("visual"); + EXPECT_EQ(nullptr, visual); + tinyxml2::XMLElement *collision = link->FirstChildElement("collision"); + EXPECT_EQ(nullptr, collision); + + // joint2_3 ignored + joint = joint->NextSiblingElement("joint"); + EXPECT_EQ(nullptr, joint); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + tinyxml2::XMLElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 8a78389e5..2be4cb607 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -53,6 +53,7 @@ set(tests unknown.cc urdf_gazebo_extensions.cc urdf_joint_parameters.cc + urdf_to_sdf.cc visual_dom.cc whitespace.cc world_dom.cc diff --git a/test/integration/deprecated_specs.cc b/test/integration/deprecated_specs.cc index 74f35d323..239fc6a58 100644 --- a/test/integration/deprecated_specs.cc +++ b/test/integration/deprecated_specs.cc @@ -61,11 +61,6 @@ TEST(DeprecatedElements, CanEmitErrors) EXPECT_EQ(sdf::ErrorCode::ELEMENT_DEPRECATED, errors[0].Code()); } -bool contains(const std::string &_a, const std::string &_b) -{ - return _a.find(_b) != std::string::npos; -} - //////////////////////////////////////////////////// TEST(DeprecatedElements, CanEmitWarningWithErrorEnforcmentPolicy) { @@ -98,7 +93,8 @@ TEST(DeprecatedElements, CanEmitWarningWithErrorEnforcmentPolicy) EXPECT_TRUE(sdf::readString( "", config, sdf, errors)); EXPECT_TRUE(errors.empty()); - EXPECT_PRED2(contains, buffer.str(), "SDF Element[testElem] is deprecated"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "SDF Element[testElem] is deprecated"); } // Flip the order of calling SetDeprecatedElementsPolicy and // SetWarningsPolicy. @@ -112,7 +108,8 @@ TEST(DeprecatedElements, CanEmitWarningWithErrorEnforcmentPolicy) EXPECT_TRUE(sdf::readString( "", config, sdf, errors)); EXPECT_TRUE(errors.empty()); - EXPECT_PRED2(contains, buffer.str(), "SDF Element[testElem] is deprecated"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "SDF Element[testElem] is deprecated"); } // Test ResetDeprecatedElementsPolicy { diff --git a/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf b/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf new file mode 100644 index 000000000..3c30afbc2 --- /dev/null +++ b/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 1 + 100.0 + 1 + + child + + + + + + diff --git a/test/integration/invalid_force_torque_sensor_massless_child_link.urdf b/test/integration/invalid_force_torque_sensor_massless_child_link.urdf new file mode 100644 index 000000000..6e4a8093e --- /dev/null +++ b/test/integration/invalid_force_torque_sensor_massless_child_link.urdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 1 + 100.0 + 1 + + child + + + + + + diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index 36eaded2d..47ffc9a3e 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -206,12 +206,6 @@ TEST(Pose1_9, PoseStringOutput) EXPECT_EQ("0 0 0 0 0 0 1", strOutput); } -////////////////////////////////////////////////// -static bool contains(const std::string &_a, const std::string &_b) -{ - return _a.find(_b) != std::string::npos; -} - ////////////////////////////////////////////////// TEST(Pose1_9, BadModelPoses) { @@ -246,7 +240,7 @@ TEST(Pose1_9, BadModelPoses) EXPECT_FALSE(sdf::readString(stream.str(), sdfParsed, errors)); EXPECT_FALSE(errors.empty()); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Undefined attribute //pose[@rotation_format='bad_rotation_format']"); } @@ -269,7 +263,7 @@ TEST(Pose1_9, BadModelPoses) EXPECT_FALSE(sdf::readString(stream.str(), sdfParsed, errors)); EXPECT_FALSE(errors.empty()); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "//pose[@rotation_format='euler_rpy'] must have 6 values, " "but 3 were found"); } @@ -293,7 +287,7 @@ TEST(Pose1_9, BadModelPoses) EXPECT_FALSE(sdf::readString(stream.str(), sdfParsed, errors)); EXPECT_FALSE(errors.empty()); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "//pose[@rotation_format='quat_xyzw'] must have 7 values, " "but 4 were found"); } @@ -317,7 +311,7 @@ TEST(Pose1_9, BadModelPoses) EXPECT_FALSE(sdf::readString(stream.str(), sdfParsed, errors)); EXPECT_FALSE(errors.empty()); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "//pose[@degrees='true'] does not apply when parsing quaternions"); } } @@ -352,7 +346,7 @@ TEST(Pose1_9, PoseSet8ValuesFail) ASSERT_NE(nullptr, poseValueParam); EXPECT_FALSE(poseValueParam->SetFromString( "1 2 3 0.7071068 0.7071068 0 0 0")); - EXPECT_PRED2(contains, buffer.str(), + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "The value for //pose[@rotation_format='quat_xyzw'] must have 7 values, " "but more than that were found in '1 2 3 0.7071068 0.7071068 0 0 0'"); } @@ -765,7 +759,7 @@ TEST(Pose1_9, ToStringWithoutAttrib) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); } ////////////////////////////////////////////////// @@ -787,8 +781,8 @@ TEST(Pose1_9, ToStringWithDegreesFalse) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='false'"); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "degrees='false'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); } ////////////////////////////////////////////////// @@ -810,8 +804,8 @@ TEST(Pose1_9, ToStringWithDegreesTrue) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='true'"); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "degrees='true'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); } ////////////////////////////////////////////////// @@ -834,8 +828,8 @@ TEST(Pose1_9, ToStringWithEulerRPY) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "rotation_format='euler_rpy'"); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "rotation_format='euler_rpy'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); } ////////////////////////////////////////////////// @@ -862,9 +856,9 @@ TEST(Pose1_9, ToStringWithEulerRPYDegreesTrue) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='true'"); - EXPECT_PRED2(contains, elemStr, "rotation_format='euler_rpy'"); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "degrees='true'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "rotation_format='euler_rpy'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); } ////////////////////////////////////////////////// @@ -889,8 +883,8 @@ TEST(Pose1_9, ToStringWithQuatXYZ) // The string output has changed as it was parsed from the value, instead of // the original string. std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); - EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "rotation_format='quat_xyzw'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.707107 0 0 0.707107"); } ////////////////////////////////////////////////// @@ -919,9 +913,9 @@ TEST(Pose1_9, ToStringWithQuatXYZWDegreesFalse) // The string output has changed as it was parsed from the value, instead of // the original string. std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='false'"); - EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); - EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "degrees='false'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "rotation_format='quat_xyzw'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.707107 0 0 0.707107"); } ////////////////////////////////////////////////// @@ -940,7 +934,7 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) ASSERT_TRUE(valParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); // Changing to attribute to degrees, however this does not modify the // value of the underlying Param. Reparse needs to be called, which uses @@ -951,14 +945,14 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) EXPECT_TRUE(valParam->Reparse()); elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='true'"); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "degrees='true'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); // Changing back to radians ASSERT_TRUE(degreesAttrib->Set(false)); elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='false'"); - EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "degrees='false'"); + EXPECT_PRED2(sdf::testing::contains, elemStr, "0.4 0.5 0.6"); } ////////////////////////////////////////////////// diff --git a/test/integration/urdf_to_sdf.cc b/test/integration/urdf_to_sdf.cc new file mode 100644 index 000000000..ceca0f197 --- /dev/null +++ b/test/integration/urdf_to_sdf.cc @@ -0,0 +1,1047 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include "sdf/sdf.hh" +#include "sdf/parser.hh" +#include "sdf/ParserConfig.hh" +#include "test_config.h" +#include "test_utils.hh" + +////////////////////////////////////////////////// +TEST(URDF2SDF, ValidBasicURDF) +{ + std::string urdfXml = R"( + + + + + + + + + )"; + + sdf::ParserConfig config; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml, config); + ASSERT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ValidContinuousJointedURDF) +{ + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::ParserConfig config; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml, config); + ASSERT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_TRUE(model->LinkNameExists("link2")); + EXPECT_TRUE(model->JointNameExists("joint1_2")); +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ZeroMassIntermediateLinkWithFixedParentJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // joint lumping and reduction occurs + { + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.LoadSdfString(urdfXml, defaultConfig); + ASSERT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // link2 visual and collision lumps into link1 + EXPECT_TRUE(link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_TRUE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + + // joint1_2 converted into frame, attached to link1 + EXPECT_TRUE(model->FrameNameExists("joint1_2")); + const sdf::Frame *frame = model->FrameByName("joint1_2"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link1"), frame->AttachedTo()); + + // link2 converted into frame, attached to joint1_2 + EXPECT_TRUE(model->FrameNameExists("link2")); + frame = model->FrameByName("link2"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint1_2"), frame->AttachedTo()); + + // joint2_3 will change to be relative to link1 + EXPECT_TRUE(model->JointNameExists("joint2_3")); + const sdf::Joint *joint = model->JointByName("joint2_3"); + ASSERT_NE(nullptr, joint); + EXPECT_EQ(std::string("link1"), joint->ParentLinkName()); + EXPECT_EQ(std::string("link3"), joint->ChildLinkName()); + + // link3 + EXPECT_TRUE(model->LinkNameExists("link3")); + } + + // Disabling lumping using gazebo tags, fails with warnings suggesting to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.LoadSdfString(urdfXml, defaultConfig); + + // no sdf errors, however we expect warnings and ignored joints and links + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning."); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // expect everything below joint1_2 to be ignored + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + + // expect no visual or collision from link2 lumped into link1 + EXPECT_FALSE( + link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + } + + // Disabling lumping using ParserConfig::URDFPreserveFixedJoint, fails with + // warnings suggesting to set to false + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + sdf::Errors errors = root.LoadSdfString(urdfXml, config); + + // no sdf errors, however we expect warnings and ignored joints and links + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // expect everything below joint1_2 to be ignored + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + + // expect no visual or collision from link2 lumped into link1 + EXPECT_FALSE( + link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + } +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ZeroMassIntermediateLinkWithFixedChildJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs, mass of link3 lumped into link2 + // link3 and joint2_3 converted into frames + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.LoadSdfString(urdfXml, defaultConfig); + + // lumping and reduction occurs, no warnings or errors should be present + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // mass of link3 lumped into link2, link2 not converted to frame + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link2 visual and collision remain + EXPECT_TRUE(link->VisualNameExists("link2_visual")); + EXPECT_TRUE(link->CollisionNameExists("link2_collision")); + + // joint2_3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("joint2_3")); + const sdf::Frame *frame = model->FrameByName("joint2_3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link2"), frame->AttachedTo()); + + // link3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("link3")); + frame = model->FrameByName("link3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint2_3"), frame->AttachedTo()); + } + + // turn off lumping with gazebo tag, conversion fails with dropped links and + // joints, and warnings to remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.LoadSdfString(urdfXml, defaultConfig); + + // Everything beneath joint1_2 is ignored, no sdf errors, but warnings + // expected with suggestion to remove gazebo tag + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning."); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->FrameNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->FrameNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } + + // Disabling lumping using ParserConfig::URDFPreserveFixedJoint, fails with + // warnings suggesting to set to false + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + sdf::Errors errors = root.LoadSdfString(urdfXml, config); + + // Everything beneath joint1_2 is ignored, no sdf errors, but warnings + // expected with suggestion to remove gazebo tag + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->FrameNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->FrameNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassLeafLink) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.LoadSdfString(urdfXml, defaultConfig); + + // lumping and reduction occurs, no warnings should be present + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision lumped into link2 + EXPECT_TRUE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_TRUE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("joint2_3")); + const sdf::Frame *frame = model->FrameByName("joint2_3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link2"), frame->AttachedTo()); + + // link3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("link3")); + frame = model->FrameByName("link3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint2_3"), frame->AttachedTo()); + } + + // lumping and reduction turned off with gazebo tag, expect link3 and joint2_3 + // to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.LoadSdfString(urdfXml, defaultConfig); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision not lumped into link2 + EXPECT_FALSE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 and link3 ignored + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } + + // ParserConfig::URDFSetPreserveFixedJoint set to true, expect link3 and + // joint2_3 to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + sdf::Errors errors = root.LoadSdfString(urdfXml, config); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision not lumped into link2 + EXPECT_FALSE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 and link3 ignored + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, URDFConvertForceTorqueSensorModels) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // valid force torque sensor + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "force_torque_sensor.urdf"); + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.Load(sdfTestFile, defaultConfig); + EXPECT_TRUE(errors.empty()) << errors; + } + + // invalid force torque sensor, with massless child link, fixed parent joint, + // lumping and reduction occurs, no errors or warning will be emitted, but + // force torque sensor won't be able to attach to joint-converted frame, this + // is the case where users need to be wary about + // TODO(aaronchongth): To provide warning when joint is dropped/converted + // during lumping and reduction + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile( + "integration", + "invalid_force_torque_sensor_lumped_and_reduced.urdf"); + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.Load(sdfTestFile, defaultConfig); + + // lumping and reduction occurs, we expect no warnings or errors + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link_1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link_1] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("base_link")); + EXPECT_TRUE(model->FrameNameExists("link_1")); + EXPECT_TRUE(model->FrameNameExists("joint_1")); + EXPECT_TRUE(model->LinkNameExists("link_2")); + EXPECT_TRUE(model->JointNameExists("joint_2")); + } + + // invalid force torque sensor, with massless child link, revolute parent + // joint, there will be warnings with joint_1 and link_1 ignored + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile( + "integration", + "invalid_force_torque_sensor_massless_child_link.urdf"); + sdf::Root root; + sdf::ParserConfig defaultConfig; + sdf::Errors errors = root.Load(sdfTestFile, defaultConfig); + + // lumping and reduction does not occur, joint_1 and link_1 ignored + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link_1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint_1] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link_1] is not modeled in sdf"); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("base_link")); + EXPECT_TRUE(model->LinkNameExists("link_2")); + EXPECT_TRUE(model->JointNameExists("joint_2")); + EXPECT_FALSE(model->FrameNameExists("link_1")); + EXPECT_FALSE(model->LinkNameExists("link_1")); + EXPECT_FALSE(model->FrameNameExists("joint_1")); + EXPECT_FALSE(model->JointNameExists("joint_1")); + } +} diff --git a/test/test_utils.hh b/test/test_utils.hh index df8e9c43f..0a8031955 100644 --- a/test/test_utils.hh +++ b/test/test_utils.hh @@ -126,6 +126,18 @@ bool LoadSdfFile(const std::string &_fileName, sdf::Root &_root) return true; } +/// \brief Checks that string _a contains string _b +inline bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +/// \brief Check that string _a does not contain string _b +inline bool notContains(const std::string &_a, const std::string &_b) +{ + return !contains(_a, _b);; +} + } // namespace testing } // namespace sdf