From c25c7c61528068febcc6463160b238a3e36270a0 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Tue, 7 Jun 2022 16:57:40 -0700 Subject: [PATCH 1/9] universal/ball -> floating joints Signed-off-by: Dharini Dutia --- sdformat_urdf/src/sdformat_urdf.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp index 3dd95aa5..ae81494c 100644 --- a/sdformat_urdf/src/sdformat_urdf.cpp +++ b/sdformat_urdf/src/sdformat_urdf.cpp @@ -493,12 +493,16 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) case sdf::JointType::PRISMATIC: urdf_joint->type = urdf::Joint::PRISMATIC; break; + case sdf::JointType::UNIVERSAL: + urdf_joint->type = urdf::Joint::FLOATING; + break; + case sdf::JointType::BALL: + urdf_joint->type = urdf::Joint::FLOATING; + break; case sdf::JointType::INVALID: // Unsupported: fall through to default - case sdf::JointType::BALL: // | case sdf::JointType::GEARBOX: // | case sdf::JointType::REVOLUTE2: // | - case sdf::JointType::SCREW: // | - case sdf::JointType::UNIVERSAL: // V + case sdf::JointType::SCREW: // V default: errors.emplace_back( sdf::ErrorCode::STRING_READ, From 50503509f11baf7837f5d2d906332d840475ffb4 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 13 Jun 2022 13:13:08 -0700 Subject: [PATCH 2/9] suggested changes Signed-off-by: Dharini Dutia --- sdformat_urdf/src/sdformat_urdf.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp index 11101f6e..3c61a178 100644 --- a/sdformat_urdf/src/sdformat_urdf.cpp +++ b/sdformat_urdf/src/sdformat_urdf.cpp @@ -494,16 +494,14 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) case sdf::JointType::PRISMATIC: urdf_joint->type = urdf::Joint::PRISMATIC; break; - case sdf::JointType::UNIVERSAL: - urdf_joint->type = urdf::Joint::FLOATING; - break; - case sdf::JointType::BALL: - urdf_joint->type = urdf::Joint::FLOATING; - break; - case sdf::JointType::INVALID: // Unsupported: fall through to default + case sdf::JointType::UNIVERSAL: // Unsupported: fall through to floating + case sdf::JointType::BALL: // Will require custom TF publisher case sdf::JointType::GEARBOX: // | case sdf::JointType::REVOLUTE2: // | case sdf::JointType::SCREW: // V + urdf_joint->type = urdf::Joint::FLOATING; + break; + case sdf::JointType::INVALID: default: errors.emplace_back( sdf::ErrorCode::STRING_READ, From 6140c00fa79f51179b12427e0cb53191323c8dfb Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Wed, 15 Jun 2022 17:18:15 -0700 Subject: [PATCH 3/9] added joint tests Signed-off-by: Dharini Dutia --- sdformat_urdf/test/joint_tests.cpp | 80 ++++++++++++++++++++++++++---- 1 file changed, 70 insertions(+), 10 deletions(-) diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp index 91e917f4..58494ecf 100644 --- a/sdformat_urdf/test/joint_tests.cpp +++ b/sdformat_urdf/test/joint_tests.cpp @@ -28,9 +28,21 @@ TEST(Joint, joint_ball) sdf::Errors errors; urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( get_file(PATH_TO_SDF_JOINT_BALL), errors); - EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_NO_ALGORITHM_ERRORS(errors); - ASSERT_FALSE(model); + ASSERT_TRUE(model); + ASSERT_EQ("joint_ball", model->getName()); + + urdf::JointConstSharedPtr joint = model->getJoint("joint_ball"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("joint_ball", joint->name); + EXPECT_EQ(urdf::Joint::FLOATING, joint->type); + ASSERT_EQ(nullptr, joint->dynamics); + ASSERT_EQ(nullptr, joint->limits); + ASSERT_EQ(nullptr, joint->safety); + ASSERT_EQ(nullptr, joint->calibration); + ASSERT_EQ(nullptr, joint->mimic); } TEST(Joint, joint_continuous) @@ -85,9 +97,21 @@ TEST(Joint, joint_gearbox) sdf::Errors errors; urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( get_file(PATH_TO_SDF_JOINT_GEARBOX), errors); - EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_NO_ALGORITHM_ERRORS(errors); - ASSERT_FALSE(model); + ASSERT_TRUE(model); + ASSERT_EQ("joint_gearbox", model->getName()); + + urdf::JointConstSharedPtr joint = model->getJoint("joint_gearbox"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("joint_gearbox", joint->name); + EXPECT_EQ(urdf::Joint::FLOATING, joint->type); + ASSERT_EQ(nullptr, joint->dynamics); + ASSERT_EQ(nullptr, joint->limits); + ASSERT_EQ(nullptr, joint->safety); + ASSERT_EQ(nullptr, joint->calibration); + ASSERT_EQ(nullptr, joint->mimic); } TEST(Joint, joint_prismatic) @@ -152,9 +176,21 @@ TEST(Joint, joint_revolute2) sdf::Errors errors; urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( get_file(PATH_TO_SDF_JOINT_REVOLUTE2), errors); - EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_NO_ALGORITHM_ERRORS(errors); - ASSERT_FALSE(model); + ASSERT_TRUE(model); + ASSERT_EQ("joint_revolute2", model->getName()); + + urdf::JointConstSharedPtr joint = model->getJoint("joint_revolute2"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("joint_revolute2", joint->name); + EXPECT_EQ(urdf::Joint::FLOATING, joint->type); + ASSERT_EQ(nullptr, joint->dynamics); + ASSERT_EQ(nullptr, joint->limits); + ASSERT_EQ(nullptr, joint->safety); + ASSERT_EQ(nullptr, joint->calibration); + ASSERT_EQ(nullptr, joint->mimic); } TEST(Joint, joint_revolute_axis) @@ -244,9 +280,21 @@ TEST(Joint, joint_screw) sdf::Errors errors; urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( get_file(PATH_TO_SDF_JOINT_SCREW), errors); - EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_NO_ALGORITHM_ERRORS(errors); - ASSERT_FALSE(model); + ASSERT_TRUE(model); + ASSERT_EQ("joint_screw", model->getName()); + + urdf::JointConstSharedPtr joint = model->getJoint("joint_screw"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("joint_screw", joint->name); + EXPECT_EQ(urdf::Joint::FLOATING, joint->type); + ASSERT_EQ(nullptr, joint->dynamics); + ASSERT_EQ(nullptr, joint->limits); + ASSERT_EQ(nullptr, joint->safety); + ASSERT_EQ(nullptr, joint->calibration); + ASSERT_EQ(nullptr, joint->mimic); } TEST(Joint, joint_universal) @@ -254,7 +302,19 @@ TEST(Joint, joint_universal) sdf::Errors errors; urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( get_file(PATH_TO_SDF_JOINT_UNIVERSAL), errors); - EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_NO_ALGORITHM_ERRORS(errors); - ASSERT_FALSE(model); + ASSERT_TRUE(model); + ASSERT_EQ("joint_universal", model->getName()); + + urdf::JointConstSharedPtr joint = model->getJoint("joint_universal"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("joint_universal", joint->name); + EXPECT_EQ(urdf::Joint::FLOATING, joint->type); + ASSERT_EQ(nullptr, joint->dynamics); + ASSERT_EQ(nullptr, joint->limits); + ASSERT_EQ(nullptr, joint->safety); + ASSERT_EQ(nullptr, joint->calibration); + ASSERT_EQ(nullptr, joint->mimic); } From 549a96342bee0a5af821abff59da1f8683370327 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 17 Jun 2022 10:03:50 -0700 Subject: [PATCH 4/9] fixed tests Signed-off-by: Dharini Dutia --- sdformat_urdf/src/sdformat_urdf.cpp | 4 ++-- sdformat_urdf/test/joint_tests.cpp | 18 +++--------------- 2 files changed, 5 insertions(+), 17 deletions(-) diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp index 3c61a178..e2ff13cd 100644 --- a/sdformat_urdf/src/sdformat_urdf.cpp +++ b/sdformat_urdf/src/sdformat_urdf.cpp @@ -509,8 +509,8 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) return nullptr; } - if (urdf::Joint::FIXED != urdf_joint->type) { - // Add axis info for non-fixed joints + if ((urdf::Joint::FIXED != urdf_joint->type) && (urdf::Joint::FLOATING != urdf_joint->type)) { + // Add axis info for non-fixed and non-floating joints const sdf::JointAxis * sdf_axis = sdf_joint.Axis(0); // URDF expects axis to be expressed in the joint frame diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp index 58494ecf..f2b8c570 100644 --- a/sdformat_urdf/test/joint_tests.cpp +++ b/sdformat_urdf/test/joint_tests.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. - +#include #include #include #include @@ -97,21 +97,9 @@ TEST(Joint, joint_gearbox) sdf::Errors errors; urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( get_file(PATH_TO_SDF_JOINT_GEARBOX), errors); - EXPECT_TRUE(errors.empty()) << errors; + EXPECT_FALSE(errors.empty()); EXPECT_NO_ALGORITHM_ERRORS(errors); - ASSERT_TRUE(model); - ASSERT_EQ("joint_gearbox", model->getName()); - - urdf::JointConstSharedPtr joint = model->getJoint("joint_gearbox"); - ASSERT_NE(nullptr, joint); - - EXPECT_EQ("joint_gearbox", joint->name); - EXPECT_EQ(urdf::Joint::FLOATING, joint->type); - ASSERT_EQ(nullptr, joint->dynamics); - ASSERT_EQ(nullptr, joint->limits); - ASSERT_EQ(nullptr, joint->safety); - ASSERT_EQ(nullptr, joint->calibration); - ASSERT_EQ(nullptr, joint->mimic); + ASSERT_FALSE(model); } TEST(Joint, joint_prismatic) From 6c17e8ac0c205f9c034dc21477d459c33ff9db5e Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 17 Jun 2022 10:24:55 -0700 Subject: [PATCH 5/9] cleaning Signed-off-by: Dharini Dutia --- sdformat_urdf/test/joint_tests.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp index f2b8c570..647b803f 100644 --- a/sdformat_urdf/test/joint_tests.cpp +++ b/sdformat_urdf/test/joint_tests.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include From a2b3dc3a422ebef8671ef6ab0fdb79b3b4449d5d Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 17 Jun 2022 14:43:40 -0700 Subject: [PATCH 6/9] check for joint axis and added test Signed-off-by: Dharini Dutia --- .../joint_prismatic_no_axis.sdf | 65 +++++++++++++++++++ .../joint_prismatic_no_axis/model.config | 19 ++++++ sdformat_urdf/CMakeLists.txt | 1 + sdformat_urdf/src/sdformat_urdf.cpp | 6 ++ sdformat_urdf/test/joint_tests.cpp | 9 +++ sdformat_urdf/test/sdf_paths.hpp.in | 1 + 6 files changed, 101 insertions(+) create mode 100644 sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf create mode 100644 sdformat_test_files/models/joint_prismatic_no_axis/model.config diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf b/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf new file mode 100644 index 00000000..39c17575 --- /dev/null +++ b/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf @@ -0,0 +1,65 @@ + + + + + + + + 0.1 0.2 0.4 + + + + + + + 0.1 0.2 0.4 + + + + + 12.3 + + 0.205 + 0 + 0 + 0.17425 + 0 + 0.05125 + + + + + 0.1 0 0.1 0 0 0 + + + + 0.1 0.2 0.3 + + + + + + + 0.1 0.2 0.3 + + + + + 1.23 + + 0.013325 + 0 + 0 + 0.01025 + 0 + 0.005125 + + + + + + link_1 + link_2 + + + diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/model.config b/sdformat_test_files/models/joint_prismatic_no_axis/model.config new file mode 100644 index 00000000..f5385be1 --- /dev/null +++ b/sdformat_test_files/models/joint_prismatic_no_axis/model.config @@ -0,0 +1,19 @@ + + + joint_prismatic_no_axis + 1.0 + joint_prismatic_no_axis.sdf + + + Shane Loretz + sloretz@openrobotics.org + + + Dharini Dutia + dharini@openrobotics.org + + + + A model with two links connected by a prismatic joint having no axis described. + + \ No newline at end of file diff --git a/sdformat_urdf/CMakeLists.txt b/sdformat_urdf/CMakeLists.txt index c5ec1faa..d2e87211 100644 --- a/sdformat_urdf/CMakeLists.txt +++ b/sdformat_urdf/CMakeLists.txt @@ -101,6 +101,7 @@ if(BUILD_TESTING) sdformat_test_files_get_model_sdf("path_to_sdf_joint_fixed" "joint_fixed") sdformat_test_files_get_model_sdf("path_to_sdf_joint_gearbox" "joint_gearbox") sdformat_test_files_get_model_sdf("path_to_sdf_joint_prismatic" "joint_prismatic") + sdformat_test_files_get_model_sdf("path_to_sdf_joint_prismatic_no_axis" "joint_prismatic_no_axis") sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute" "joint_revolute") sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute2" "joint_revolute2") sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute_axis" "joint_revolute_axis") diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp index e2ff13cd..fec7ece7 100644 --- a/sdformat_urdf/src/sdformat_urdf.cpp +++ b/sdformat_urdf/src/sdformat_urdf.cpp @@ -512,6 +512,12 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) if ((urdf::Joint::FIXED != urdf_joint->type) && (urdf::Joint::FLOATING != urdf_joint->type)) { // Add axis info for non-fixed and non-floating joints const sdf::JointAxis * sdf_axis = sdf_joint.Axis(0); + if(nullptr == sdf_axis) { + errors.emplace_back( + sdf::ErrorCode::STRING_READ, + "Axes missing for joint [" + sdf_joint.Name() + "]"); + return nullptr; + } // URDF expects axis to be expressed in the joint frame ignition::math::Vector3d axis_xyz; diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp index 647b803f..afc36b09 100644 --- a/sdformat_urdf/test/joint_tests.cpp +++ b/sdformat_urdf/test/joint_tests.cpp @@ -131,6 +131,15 @@ TEST(Joint, joint_prismatic) ASSERT_EQ(nullptr, joint->mimic); } +TEST(Joint, joint_prismatic_no_axis) +{ + sdf::Errors errors; + urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse( + get_file(PATH_TO_SDF_JOINT_PRISMATIC_NO_AXIS), errors); + EXPECT_FALSE(errors.empty()); + ASSERT_FALSE(model); +} + TEST(Joint, joint_revolute) { sdf::Errors errors; diff --git a/sdformat_urdf/test/sdf_paths.hpp.in b/sdformat_urdf/test/sdf_paths.hpp.in index 75f62bfd..8cae9fc0 100644 --- a/sdformat_urdf/test/sdf_paths.hpp.in +++ b/sdformat_urdf/test/sdf_paths.hpp.in @@ -35,6 +35,7 @@ #define PATH_TO_SDF_JOINT_FIXED "@path_to_sdf_joint_fixed@" #define PATH_TO_SDF_JOINT_GEARBOX "@path_to_sdf_joint_gearbox@" #define PATH_TO_SDF_JOINT_PRISMATIC "@path_to_sdf_joint_prismatic@" +#define PATH_TO_SDF_JOINT_PRISMATIC_NO_AXIS "@path_to_sdf_joint_prismatic_no_axis@" #define PATH_TO_SDF_JOINT_REVOLUTE "@path_to_sdf_joint_revolute@" #define PATH_TO_SDF_JOINT_REVOLUTE2 "@path_to_sdf_joint_revolute2@" #define PATH_TO_SDF_JOINT_REVOLUTE_AXIS "@path_to_sdf_joint_revolute_axis@" From 8f1f2063444491013d75b2fc7c6eec1371bddb46 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 17 Jun 2022 14:58:37 -0700 Subject: [PATCH 7/9] newline Signed-off-by: Dharini Dutia --- sdformat_test_files/models/joint_prismatic_no_axis/model.config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/model.config b/sdformat_test_files/models/joint_prismatic_no_axis/model.config index f5385be1..2ceb5977 100644 --- a/sdformat_test_files/models/joint_prismatic_no_axis/model.config +++ b/sdformat_test_files/models/joint_prismatic_no_axis/model.config @@ -16,4 +16,4 @@ A model with two links connected by a prismatic joint having no axis described. - \ No newline at end of file + From 5b2fdf78a36a6323bc6ceba4e6a0048fd6b40f88 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 17 Jun 2022 15:57:12 -0700 Subject: [PATCH 8/9] updating CMakeLists Signed-off-by: Dharini Dutia --- sdformat_test_files/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/sdformat_test_files/CMakeLists.txt b/sdformat_test_files/CMakeLists.txt index 66e87ffe..510bde71 100644 --- a/sdformat_test_files/CMakeLists.txt +++ b/sdformat_test_files/CMakeLists.txt @@ -34,6 +34,7 @@ set(model_names "joint_fixed" "joint_gearbox" "joint_prismatic" + "joint_prismatic_no_axis" "joint_revolute" "joint_revolute2" "joint_revolute_axis" From b69f03cd49cf0ab2a39b4c97eedab6fbe7723f28 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 17 Jun 2022 16:07:10 -0700 Subject: [PATCH 9/9] missing space Signed-off-by: Dharini Dutia --- sdformat_urdf/src/sdformat_urdf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp index fec7ece7..0bca5841 100644 --- a/sdformat_urdf/src/sdformat_urdf.cpp +++ b/sdformat_urdf/src/sdformat_urdf.cpp @@ -512,7 +512,7 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) if ((urdf::Joint::FIXED != urdf_joint->type) && (urdf::Joint::FLOATING != urdf_joint->type)) { // Add axis info for non-fixed and non-floating joints const sdf::JointAxis * sdf_axis = sdf_joint.Axis(0); - if(nullptr == sdf_axis) { + if (nullptr == sdf_axis) { errors.emplace_back( sdf::ErrorCode::STRING_READ, "Axes missing for joint [" + sdf_joint.Name() + "]");