Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support for universal and ball joint in sdf #13

Merged
merged 10 commits into from
Jun 20, 2022
Merged

Support for universal and ball joint in sdf #13

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
c25c7c6
universal/ball -> floating joints
quarkytale Jun 7, 2022
64ce17d
Merge branch 'ros2' into quarkytale/joint_support
quarkytale Jun 13, 2022
5050350
suggested changes
quarkytale Jun 13, 2022
6140c00
added joint tests
quarkytale Jun 16, 2022
549a963
fixed tests
quarkytale Jun 17, 2022
6c17e8a
cleaning
quarkytale Jun 17, 2022
a2b3dc3
check for joint axis and added test
quarkytale Jun 17, 2022
8f1f206
newline
quarkytale Jun 17, 2022
5b2fdf7
updating CMakeLists
quarkytale Jun 17, 2022
b69f03c
missing space
quarkytale Jun 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 8 additions & 6 deletions sdformat_urdf/src/sdformat_urdf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -494,21 +494,23 @@ 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::INVALID: // Unsupported: fall through to default
case sdf::JointType::BALL: // |
case sdf::JointType::UNIVERSAL: // Unsupported: fall through to floating
case sdf::JointType::BALL: // Will require custom TF publisher
case sdf::JointType::GEARBOX: // |
quarkytale marked this conversation as resolved.
Show resolved Hide resolved
case sdf::JointType::REVOLUTE2: // |
case sdf::JointType::SCREW: // |
case sdf::JointType::UNIVERSAL: // V
case sdf::JointType::SCREW: // V
urdf_joint->type = urdf::Joint::FLOATING;
break;
case sdf::JointType::INVALID:
default:
errors.emplace_back(
sdf::ErrorCode::STRING_READ,
"Unsupported joint type on joint [" + sdf_joint.Name() + "]");
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);
chapulina marked this conversation as resolved.
Show resolved Hide resolved

// URDF expects axis to be expressed in the joint frame
Expand Down
65 changes: 56 additions & 9 deletions sdformat_urdf/test/joint_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.


#include <gtest/gtest.h>
#include <urdf_model/model.h>
#include <urdf_model/types.h>
Expand All @@ -28,9 +27,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);
Copy link
Contributor

@chapulina chapulina Jun 17, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think lines 40~44 can be EXPECT_EQ, because we're not trying to access those pointers in the test anyway. This way, if in the future one of them fails, we will know whether the others fail too.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree, though this could already be improved about the existing tests in this file so I wouldn't block on it.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I can work on this in a separate PR :)

Copy link
Contributor Author

@quarkytale quarkytale Jun 24, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Opened #16 for improving tests.

}

TEST(Joint, joint_continuous)
Expand Down Expand Up @@ -152,9 +163,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)
Expand Down Expand Up @@ -244,17 +267,41 @@ 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)
{
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);
}