-
Notifications
You must be signed in to change notification settings - Fork 65
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Try to fix urdfs in memory before SDF import (#481)
* Fix in memory urdf with errors 2 and 19 * Notify user and added settings * Apply suggestions from code review Co-authored-by: Adam Dąbrowski <[email protected]> Co-authored-by: lumberyard-employee-dm <[email protected]> Co-authored-by: Mike Balfour <[email protected]> Signed-off-by: Michał Pełka <[email protected]>
- Loading branch information
4 people
committed
Oct 3, 2023
1 parent
d45ccc6
commit b19dea8
Showing
12 changed files
with
265 additions
and
35 deletions.
There are no files selected for viewing
129 changes: 129 additions & 0 deletions
129
Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,129 @@ | ||
| /* | ||
| * Copyright (c) Contributors to the Open 3D Engine Project. | ||
| * For complete copyright and license terms please see the LICENSE at the root of this distribution. | ||
| * | ||
| * SPDX-License-Identifier: Apache-2.0 OR MIT | ||
| * | ||
| */ | ||
|
|
||
| #include "FixURDF.h" | ||
| #include <AzCore/XML/rapidxml.h> | ||
| #include <AzCore/XML/rapidxml_print.h> | ||
| #include <AzCore/std/containers/unordered_map.h> | ||
| #include <iostream> | ||
|
|
||
| namespace ROS2::Utils | ||
| { | ||
| //! Modifies a parsed URDF in memory to add missing inertia to links, which prevents SDF error 19. | ||
| //! @param urdf URDF to modify. | ||
| //! @returns a list of names of links that were modified. | ||
| AZStd::vector<AZStd::string> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf) | ||
| { | ||
| AZStd::vector<AZStd::string> modifiedLinks; | ||
| using namespace AZ::rapidxml; | ||
| AZStd::vector<xml_node<>*> links; | ||
| for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link")) | ||
| { | ||
| if (!link->first_node("inertial")) | ||
| { | ||
| links.push_back(link); | ||
| } | ||
| } | ||
|
|
||
| for (auto* link : links) | ||
| { | ||
| xml_node<>* inertial = urdf->document()->allocate_node(node_element, "inertial"); | ||
|
|
||
| xml_node<>* mass = urdf->document()->allocate_node(node_element, "mass"); | ||
| mass->append_attribute(urdf->document()->allocate_attribute("value", "1.")); | ||
| inertial->append_node(mass); | ||
| xml_node<>* inertia = urdf->document()->allocate_node(node_element, "inertia"); | ||
|
|
||
| inertia->append_attribute(urdf->document()->allocate_attribute("ixx", "1.")); | ||
| inertia->append_attribute(urdf->document()->allocate_attribute("ixy", "0.")); | ||
| inertia->append_attribute(urdf->document()->allocate_attribute("ixz", "0.")); | ||
|
|
||
| inertia->append_attribute(urdf->document()->allocate_attribute("iyx", "0.")); | ||
| inertia->append_attribute(urdf->document()->allocate_attribute("iyy", "1.")); | ||
| inertia->append_attribute(urdf->document()->allocate_attribute("iyz", "0.")); | ||
|
|
||
| inertia->append_attribute(urdf->document()->allocate_attribute("izx", "0.")); | ||
| inertia->append_attribute(urdf->document()->allocate_attribute("izy", "0.")); | ||
| inertia->append_attribute(urdf->document()->allocate_attribute("izz", "1.")); | ||
|
|
||
| inertial->append_node(inertia); | ||
|
|
||
| xml_node<>* origin = urdf->document()->allocate_node(node_element, "origin"); | ||
| origin->append_attribute(urdf->document()->allocate_attribute("xyz", "0. 0. 0.")); | ||
| inertial->append_node(origin); | ||
|
|
||
| auto* name = link->first_attribute("name"); | ||
| if (name) | ||
| { | ||
| modifiedLinks.push_back(name->value()); | ||
| } | ||
| link->append_node(inertial); | ||
| } | ||
|
|
||
| return modifiedLinks; | ||
| } | ||
|
|
||
| //! Handles a case of multiple joints and the link sharing a common names which causes SDF error2 (but is fine in URDF) | ||
| //! Function will add a suffix "_dup" to the name of the joint if it is also the name of a link. | ||
| //! If there are name collisions in links, this will not be able to fix it, the SDF parser will throw an error. | ||
| //! @param urdf URDF to modify. | ||
| //! @returns a list of links that were modified | ||
| AZStd::vector<AZStd::string> RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf) | ||
| { | ||
| using namespace AZ::rapidxml; | ||
| AZStd::vector<AZStd::string> modifiedLinks; | ||
| AZStd::unordered_map<AZStd::string, unsigned int> linkAndJointsName; | ||
| for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link")) | ||
| { | ||
| auto* name = link->first_attribute("name"); | ||
| if (name) | ||
| { | ||
| linkAndJointsName.insert(AZStd::make_pair(name->value(), 0)); | ||
| } | ||
| } | ||
| for (xml_node<>* joint = urdf->first_node("joint"); joint; joint = joint->next_sibling("joint")) | ||
| { | ||
| auto* name = joint->first_attribute("name"); | ||
| if (name) | ||
| { | ||
| if (linkAndJointsName.contains(name->value())) | ||
| { | ||
| unsigned int& count = linkAndJointsName[name->value()]; | ||
| auto newName = AZStd::string::format("%s_dup%u", name->value(), count); | ||
| name->value(urdf->document()->allocate_string(newName.c_str())); | ||
| count++; | ||
| modifiedLinks.push_back(AZStd::move(newName)); | ||
| } | ||
| else | ||
| { | ||
| linkAndJointsName.insert(AZStd::make_pair(name->value(), 0)); | ||
| } | ||
| } | ||
| } | ||
| return modifiedLinks; | ||
| } | ||
|
|
||
| AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const std::string& data) | ||
| { | ||
| AZStd::vector<AZStd::string> modifiedElements; | ||
| using namespace AZ::rapidxml; | ||
| xml_document<> doc; | ||
| doc.parse<0>(const_cast<char*>(data.c_str())); | ||
| xml_node<>* urdf = doc.first_node("robot"); | ||
| auto links = AddMissingInertiaToLinks(urdf); | ||
| modifiedElements.insert(modifiedElements.end(), AZStd::make_move_iterator(links.begin()), AZStd::make_move_iterator(links.end())); | ||
|
|
||
| auto renames = RenameDuplicatedJoints(urdf); | ||
| modifiedElements.insert( | ||
| modifiedElements.end(), AZStd::make_move_iterator(renames.begin()), AZStd::make_move_iterator(renames.end())); | ||
|
|
||
| std::string xmlDocString; | ||
| AZ::rapidxml::print(std::back_inserter(xmlDocString), *urdf, 0); | ||
| return { xmlDocString, modifiedElements }; | ||
| } | ||
| } // namespace ROS2::Utils |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,23 @@ | ||
| /* | ||
| * Copyright (c) Contributors to the Open 3D Engine Project. | ||
| * For complete copyright and license terms please see the LICENSE at the root of this distribution. | ||
| * | ||
| * SPDX-License-Identifier: Apache-2.0 OR MIT | ||
| * | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <AzCore/std/containers/vector.h> | ||
| #include <AzCore/std/string/string.h> | ||
|
|
||
| namespace ROS2::Utils | ||
| { | ||
| //! Modifies in memory URDF to increase chance of successful conversion to SDF. | ||
| //! It does the following: | ||
| //! - Adds missing inertia to links of mass 1 kg and identity inertia matrix. | ||
| //! - Renames joints that have the same name as a link. | ||
| //! @param urdf URDF to modify. | ||
| //! @returns a modified URDF and a list of XML element that were modified | ||
| AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const std::string& data); | ||
| } // namespace ROS2::Utils |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.