From b7f0cba731101eed7e3fe84d2ea4330031f89037 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 21 Sep 2022 14:21:34 -0700 Subject: [PATCH] urdf: fix test and clean up internals (#1126) The urdf_gazebo_extensions.urdf file had incorrect kinematics with link3 being the child of two different joints. This adds a link4 and replaces joint13 with joint14 connecting link1 and link4. This also fixes some expectations about the implicit_spring_damper tags. * parser_urdf: clean up internal functions by removing unused parameters and making other parameters const. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 30 +++++++------------- test/integration/urdf_gazebo_extensions.cc | 17 +++++++---- test/integration/urdf_gazebo_extensions.urdf | 30 ++++++++++++++------ 3 files changed, 42 insertions(+), 35 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 4e6772b72..dd5ad0e21 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -123,7 +123,7 @@ void CreateVisual(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link, /// create SDF Joint block based on URDF void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &_currentTransform); + const ignition::math::Pose3d &_currentTransform); /// insert extensions into links void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName); @@ -142,12 +142,11 @@ void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform); /// create SDF from URDF link -void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &_transform); +void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link); /// create SDF Link block based on URDF void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &_currentTransform); + const ignition::math::Pose3d &_currentTransform); /// reduced fixed joints: apply appropriate frame updates in joint /// inside urdf extensions when doing fixed joint reduction @@ -1025,12 +1024,10 @@ void ReduceJointsToParent(urdf::LinkSharedPtr _link) { // go down the tree until we hit a parent joint that is not fixed urdf::LinkSharedPtr newParentLink = _link; - ignition::math::Pose3d jointAnchorTransform; while (newParentLink->parent_joint && newParentLink->getParent()->name != "world" && FixedJointShouldBeReduced(newParentLink->parent_joint) ) { - jointAnchorTransform = jointAnchorTransform * jointAnchorTransform; parentJoint->parent_to_joint_origin_transform = TransformToParentFrame( parentJoint->parent_to_joint_origin_transform, @@ -2606,11 +2603,8 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) //////////////////////////////////////////////////////////////////////////////// void CreateSDF(TiXmlElement *_root, - urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &_transform) + urdf::LinkConstSharedPtr _link) { - ignition::math::Pose3d _currentTransform = _transform; - // must have an block and cannot have zero mass. // allow det(I) == zero, in the case of point mass geoms. // @todo: keyword "world" should be a constant defined somewhere else @@ -2652,13 +2646,13 @@ void CreateSDF(TiXmlElement *_root, (!_link->parent_joint || !FixedJointShouldBeReduced(_link->parent_joint))) { - CreateLink(_root, _link, _currentTransform); + CreateLink(_root, _link, ignition::math::Pose3d::Zero); } // recurse into children for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i) { - CreateSDF(_root, _link->child_links[i], _currentTransform); + CreateSDF(_root, _link->child_links[i]); } } @@ -2693,7 +2687,7 @@ urdf::Pose CopyPose(ignition::math::Pose3d _pose) //////////////////////////////////////////////////////////////////////////////// void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &_currentTransform) + const ignition::math::Pose3d &_currentTransform) { // create new body TiXmlElement *elem = new TiXmlElement("link"); @@ -2870,7 +2864,7 @@ void CreateInertial(TiXmlElement *_elem, //////////////////////////////////////////////////////////////////////////////// void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &/*_currentTransform*/) + const ignition::math::Pose3d &/*_currentTransform*/) { // compute the joint tag std::string jtype; @@ -3150,10 +3144,6 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, return sdfXmlOut; } - // initialize transform for the model, urdf is recursive, - // while sdf defines all links relative to model frame - ignition::math::Pose3d transform; - // parse sdf extension TiXmlDocument urdfXml; urdfXml.Parse(_urdfStr.c_str()); @@ -3196,13 +3186,13 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, child = rootLink->child_links.begin(); child != rootLink->child_links.end(); ++child) { - CreateSDF(robot, (*child), transform); + CreateSDF(robot, (*child)); } } else { // convert, starting from root link - CreateSDF(robot, rootLink, transform); + CreateSDF(robot, rootLink); } // insert the extensions without reference into root level diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 6b9fbcf37..e9d0a8d5b 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -86,17 +86,22 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) else if (jointName == "joint12") { // cfmDamping not provided - ASSERT_TRUE(joint->HasElement("physics")); + EXPECT_TRUE(joint->HasElement("physics")); sdf::ElementPtr physics = joint->GetElement("physics"); - EXPECT_FALSE(physics->HasElement("implicit_spring_damper")); + EXPECT_TRUE(physics->HasElement("ode")); + sdf::ElementPtr ode = physics->GetElement("ode"); + EXPECT_TRUE(ode->HasElement("implicit_spring_damper")); + EXPECT_FALSE(ode->Get("implicit_spring_damper")); } - else if (jointName == "joint13") + else if (jointName == "joint14") { // implicitSpringDamper = 1 - ASSERT_TRUE(joint->HasElement("physics")); + EXPECT_TRUE(joint->HasElement("physics")); sdf::ElementPtr physics = joint->GetElement("physics"); - ASSERT_TRUE(physics->HasElement("implicit_spring_damper")); - EXPECT_TRUE(physics->Get("implicit_spring_damper")); + EXPECT_TRUE(physics->HasElement("ode")); + sdf::ElementPtr ode = physics->GetElement("ode"); + EXPECT_TRUE(ode->HasElement("implicit_spring_damper")); + EXPECT_TRUE(ode->Get("implicit_spring_damper")); } } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index a0a92c6c7..bfbde44e2 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -95,14 +95,6 @@ 0.1 - - - - - - - - @@ -125,7 +117,27 @@ - + + + + + + + + + + + + + + + + + + + + + true