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.

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

Updating ROS2 RobotImporter to use libsdformat #460

Merged

Conversation

lemonade-dm
Copy link
Contributor

Replaced references to URDFdom with SDFormat.

Updated the Link query logic in the RobotImporter

Joints in which a link is parent can be queried using the new GetJointsForParentLink function.

Joints in which a link is child can be queried using the new GetJointsForChildLink function.

A VisitJointsForModel and VisitLinksForModel function has been added to allow visting each joint or link on model.
It supports visiting joints or links on nested models as well

Fixed the URDF Parser Test to account for link and joints changes when using libsdformat for parsing a URDF.

SDFormat URDF parser uses joint reduction to combine two links joined together by fixed joint into the parent link itself.

That operation results in the removal of the child link and the joint.
SDFormat creates a Frame for the removed child link and joint using their "name" fields, so it is possible to still discover information about the child link.

mbalfour-amzn and others added 2 commits August 16, 2023 18:29
URDF Links within the model still need to be fixed up.

Signed-off-by: lumberyard-employee-dm <[email protected]>
Joints in which a link is parent can be queried using the new `GetJointsForParentLink` function.

Joints in which a link is child can be queried using the new `GetJointsForChildLink` function.

A VisitJointsForModel and VisitLinksForModel function has been added to allow visting each joint or link on model.
It supports visiting joints or links on nested models as well

Fixed the URDF Parser Test to account for link and joints changes when using libsdformat for parsing a URDF.

SDFormat URDF parser uses joint reduction to combine two links joined together by fixed joint into the parent link itself.

That operation results in the removal of the child link and the joint.
SDFormat creates a Frame for the removed child link and joint using their "name" fields, so it is possible to still discover information about the child link.

Signed-off-by: lumberyard-employee-dm <[email protected]>
@lemonade-dm lemonade-dm force-pushed the urdf-importer-refactor branch from f8ce870 to 5b125f2 Compare August 17, 2023 00:12
Copy link
Contributor

@michalpelka michalpelka left a comment

Choose a reason for hiding this comment

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

Could not build PR with non-unity build.
AZ_Warning(SdfAssetBuilderName, !m_builderPatterns.empty(), "SdfAssetBuilder disabled, no supported file type extensions found.");
in SdfAssetBuilderSettings.cpp that happened to live in one compilation unit with SdfAssetBuilderSettings.cpp when unity build are used. Without non-unity build you cannot access this constexpr.

Copy link
Contributor

@michalpelka michalpelka left a comment

Choose a reason for hiding this comment

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

Few minor things.

{
namespace UrdfParser::Internal
void CheckIfCurrentLocaleHasDotAsADecimalSeparator()
Copy link
Contributor

@michalpelka michalpelka Aug 17, 2023

Choose a reason for hiding this comment

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

It is worth checking if SDFLib handles it correctly. If so we can remove this hack ("CheckIfCurrentLocaleHasDotAsADecimalSeparator").

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes, I will give that a try.

Copy link
Contributor Author

@lemonade-dm lemonade-dm Aug 19, 2023

Choose a reason for hiding this comment

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

I didn't have any issues parsing the chain.sdf from the valid URDF testing case when changing the locale to be French Language in the France region using fr_FR.UTF-8 after removing the code

{
private:
std::stringstream console_ss;
class CustomConsoleHandler : public console_bridge::OutputHandler
Copy link
Contributor

Choose a reason for hiding this comment

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

We should remove it, since it won't work with sdferr macro. Fortunately there is similar code in https://github.com/gazebosim/sdformat/blob/sdf13/test/test_utils.hh#L80 that allows pretty easily grab console ouput.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Actually, I don't think we need to capture the console for SDFormat, since the API for parsing an SDF returns an sdf::Error vector which contains the same errors that get sent to the sdf::Console

The ROS2::UrdfParser::Parse function actually returns those errors to the caller, which would allow them to log it how they choose: http://osrf-distributions.s3.amazonaws.com/sdformat/api/13.2.0/classsdf_1_1SDF__VERSION__NAMESPACE_1_1Root.html

@michalpelka
Copy link
Contributor

Unfortunately this contribution gives unexpected, and different results on our manual test.
Using development:
Screenshot from 2023-08-17 14-34-23
Using this PR:
Screenshot from 2023-08-17 14-32-06

Copy link
Contributor

@adamdbrw adamdbrw left a comment

Choose a reason for hiding this comment

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

Great work! @jhanca-robotecai will test this idependently.

aggregateErrorMessages += errorMessage;
aggregateErrorMessages += '\n';
}
AZ_Warning("ROS2EditorSystemComponent", false, "URDF parsing failed with errors: %s\nRefer to %s",
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
AZ_Warning("ROS2EditorSystemComponent", false, "URDF parsing failed with errors: %s\nRefer to %s",
AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "URDF parsing failed with errors: %s\nRefer to %s",

Note: This warning message source was incorrect previously.

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'll correct the window

AZStd::string prefabName = AZStd::string(parsedUrdf->getName().c_str(), parsedUrdf->getName().size()) + ".prefab";
uint64_t urdfWorldCount = parsedUrdfRoot.WorldCount();
if (urdfWorldCount == 0)
{
Copy link
Contributor

Choose a reason for hiding this comment

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

In the format itself, the world element is optional, since robots can be defined either with or without the world context (as a child element or as a sibling of world element).

image

Perhaps the converter always creates the robot inside a world element, but I am not sure whether we should rely on that it won't change. An example of correct SDF that this would skip:

<?xml version='1.0'?>
<sdf version='1.10'>
  <model name='my_model'>
    ...
  </model>
</sdf>

What do you think?

Copy link
Contributor Author

@lemonade-dm lemonade-dm Aug 17, 2023

Choose a reason for hiding this comment

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

I am thinking we could check if there is at least 1 model, otherwise the URDF wouldn't have any robot data within it.

Copy link
Contributor

Choose a reason for hiding this comment

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

IMO, I'd be inclined to skip any validation like that for now. Technically, I think the sdf format supports files with no models, like one with just plugins and lights for example. Admittedly a file without models is probably not useful and maybe should generate a warning, but it seems a little bit wrong to me to disallow it completely. I won't be opposed if you decide to require it though.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Well we use the Model name or World name for the name of prefab that gets generated.
If want we can use the name of the URDF file itself and then let the URDFPrefabMaker code take of dealing with not having any models.

Copy link
Contributor

Choose a reason for hiding this comment

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

What I mean here is that there can be Models without any Worlds. So just checking for Worlds is not enough.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yeah, I noticed that is was the case for URDF conversion.
It comes with only a root without any tags within it.

I have updated the logic, to use the name of the root tag if it exist, otherwise it uses the name of first tag as the prefab name.

uint64_t urdfWorldCount = parsedUrdfRoot.WorldCount();
if (urdfWorldCount == 0)
{
AZ_Error("ROS2EditorSystemComponent", false, "URDF file converted to SDF %s contains no worlds."
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
AZ_Error("ROS2EditorSystemComponent", false, "URDF file converted to SDF %s contains no worlds."
AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "URDF file converted to SDF %s contains no worlds."

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Will fix

between URDF and SDF coordinate frame
http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.5#parent-frames-in-urdf
> The most significant difference between URDF and SDFormat coordinate frames is related to links and joints. While SDFormat allows kinematic loops with the topology of a directed graph, URDF kinematics must have the topology of a directed tree, with each link being the child of at most one joint. URDF coordinate frames are defined recursively based on this tree structure, with each joint's <origin/> tag defining the coordinate transformation from the parent link frame to the child link frame.
*/
Copy link
Contributor

Choose a reason for hiding this comment

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

Nice addition!

}
sdfImportErrors += errorMessage;
sdfImportErrors += '\n';
}
Copy link
Contributor

Choose a reason for hiding this comment

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

This error composing block seems to be the same same as in ROS2RobotImporterEditorSystemComponent (repeated code).

One more occurrence.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

It is the same.
I'll add a function, that can convert an sdf::Errors type alias which is a std::vector<sdf::Error> into an AZStd::string

Copy link
Contributor

@jhanca-robotecai jhanca-robotecai Aug 18, 2023

Choose a reason for hiding this comment

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

The errors should be merged with xacro errors, e.g. for ur5 robot the list of errors is as follows:
Screenshot from 2023-08-18 15-39-49
while the terminal shows some meaningful outcome:

Warning [parser_urdf.cc:2774] Error Code 19: Msg: urdf2sdf: link[base_link] has no <inertial> block defined. Please ensure this link has a valid mass to prevent any missing links or joints in the resulting SDF model.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed parent joint[base_joint], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: parent joint[base_joint] ignored.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed child joint[base_link-base_fixed_joint], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed child joint[base_link-base_link_inertia], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: [2] child joints ignored.
Error Code 19: Msg: urdf2sdf: [2] child links ignored.
Warning [parser_urdf.cc:2777] Error Code 19: Msg: urdf2sdf: link[base_link] is not modeled in sdf.

O3DE console prints the following message:

(ParseXacro) - xacro executable : xacro
(ParseXacro) - Convert xacro file : /home/jhanca/devtrunk/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro
(ParseXacro) - calling file : xacro /home/jhanca/devtrunk/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro ur_type:=ur5e name:=FooRobot
(ParseXacro) - xacro finished with success

Copy link
Contributor

@jhanca-robotecai jhanca-robotecai left a comment

Choose a reason for hiding this comment

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

This is a great step towards working SDFormat. Thanks for taking the time to fix additional issues, such as wrong naming.

Unfortunately there are some problems with the link structure, e.g. valid simple urdf
image

There are also some problems with importing meshes, e.g. UR5e
image

@@ -19,7 +19,7 @@ namespace ROS2
{
m_fileDialog = new QFileDialog(this);
m_fileDialog->setDirectory(QString::fromUtf8(AZ::Utils::GetProjectPath().data()));
m_fileDialog->setNameFilter("URDF, XACRO (*.urdf *.xacro)");
m_fileDialog->setNameFilter("URDF, XACRO, SDF (*.urdf *.xacro *.sdf)");
Copy link
Contributor

Choose a reason for hiding this comment

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

This change has no effect: RobotImporterWidget::OpenUrdf method checks the extension again and ignores anything different than .xacro or .urdf

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Well this is for Widget dialog itself.

The SDF shouldn't be opened with RobotImporterWidget::OpenUrdf function, but instead a new function for opening up an SDF would be needed such as OpenSDF.

Copy link
Contributor

Choose a reason for hiding this comment

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

FYI - this should also support *.world. A "world" file is just an sdf file as far as I can tell, but that's the extension gazebo uses.

If urdf files are now getting converted internally into sdf, why would we need a new function for opening sdf files? Wouldn't we just extend the current code to handle more and more of the sdf format, and not need to differentiate between the two?

Gems/ROS2/Code/Tests/UrdfParserTest.cpp Show resolved Hide resolved
Gems/ROS2/Code/Tests/UrdfParserTest.cpp Show resolved Hide resolved
gz::math::Pose3d resolvedPose;

if (sdf::Errors poseResolveErrors = linkSemanticPos.Resolve(resolvedPose);
!poseResolveErrors.empty())
Copy link
Contributor

Choose a reason for hiding this comment

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

Please correct me if I am wrong, but while playing with this file my clang-format removed the line break here, so I assume there is a problem on either my or your side.

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 believe clang-format is combining the lines together since there are space for both the if initializer and the condition to fit under 140 characters

@jhanca-robotecai
Copy link
Contributor

Unfortunately there are some problems with the link structure, e.g. valid simple urdf

Some debug comments:

I tried to parse this file using the sdformat tutorial and the parser can see only one link (the first one) and only one joint (cannot read the name) in this file. Either the URDF parser in libsdformat is not perfect, or the file is corrupted.

Found urdf_test model!
Found box_link link in urdf_test model!
Found __default__ joint in urdf_test model!
Joint __default__ connects __default__ link to __default__ link

@lemonade-dm
Copy link
Contributor Author

Could not build PR with non-unity build. AZ_Warning(SdfAssetBuilderName, !m_builderPatterns.empty(), "SdfAssetBuilder disabled, no supported file type extensions found."); in SdfAssetBuilderSettings.cpp that happened to live in one compilation unit with SdfAssetBuilderSettings.cpp when unity build are used. Without non-unity build you cannot access this constexpr.

I'll fix this on the side.

@lemonade-dm
Copy link
Contributor Author

lemonade-dm commented Aug 17, 2023

Unfortunately there are some problems with the link structure, e.g. valid simple urdf

Some debug comments:

I tried to parse this file using the sdformat tutorial and the parser can see only one link (the first one) and only one joint (cannot read the name) in this file. Either the URDF parser in libsdformat is not perfect, or the file is corrupted.

Found urdf_test model!
Found box_link link in urdf_test model!
Found __default__ joint in urdf_test model!
Joint __default__ connects __default__ link to __default__ link

This is occurring because the SDFormat URDF parsing logic merges links that tied together by fixed joints into just a single link: gazebosim/sdformat#1110

The joint reduction can be seen here

@lemonade-dm
Copy link
Contributor Author

Unfortunately there are some problems with the link structure, e.g. valid simple urdf

Some debug comments:
I tried to parse this file using the sdformat tutorial and the parser can see only one link (the first one) and only one joint (cannot read the name) in this file. Either the URDF parser in libsdformat is not perfect, or the file is corrupted.

Found urdf_test model!
Found box_link link in urdf_test model!
Found __default__ joint in urdf_test model!
Joint __default__ connects __default__ link to __default__ link

This is occurring because the SDFormat URDF parsing logic merges links that tied together by fixed joints into just a single link: gazebosim/sdformat#1110

There is a extension tag that can be added to the URDF to preserve fixed joints that is detailed in the SDFormat documentation: http://sdformat.org/tutorials?tut=sdformat_urdf_extensions&cat=specification&#gazebo-elements-for-joints

@@ -19,7 +19,7 @@ namespace ROS2
{
m_fileDialog = new QFileDialog(this);
m_fileDialog->setDirectory(QString::fromUtf8(AZ::Utils::GetProjectPath().data()));
m_fileDialog->setNameFilter("URDF, XACRO (*.urdf *.xacro)");
m_fileDialog->setNameFilter("URDF, XACRO, SDF (*.urdf *.xacro *.sdf)");
Copy link
Contributor

Choose a reason for hiding this comment

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

FYI - this should also support *.world. A "world" file is just an sdf file as far as I can tell, but that's the extension gazebo uses.

If urdf files are now getting converted internally into sdf, why would we need a new function for opening sdf files? Wouldn't we just extend the current code to handle more and more of the sdf format, and not need to differentiate between the two?

AZStd::string prefabName = AZStd::string(parsedUrdf->getName().c_str(), parsedUrdf->getName().size()) + ".prefab";
uint64_t urdfWorldCount = parsedUrdfRoot.WorldCount();
if (urdfWorldCount == 0)
{
Copy link
Contributor

Choose a reason for hiding this comment

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

IMO, I'd be inclined to skip any validation like that for now. Technically, I think the sdf format supports files with no models, like one with just plugins and lights for example. Admittedly a file without models is probably not useful and maybe should generate a warning, but it seems a little bit wrong to me to disallow it completely. I won't be opposed if you decide to require it though.

Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp Outdated Show resolved Hide resolved
Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp Outdated Show resolved Hide resolved
Comment on lines 98 to 107
// Check if name is catchy for wheel
if (!AZStd::regex_search(link_name, match, wheel_regex))
if (!AZStd::regex_search(linkName, match, wheelRegex))
{
return false;
}
// The name should contain a joint word
if (AZStd::regex_search(link_name, match, joint_regex))
if (AZStd::regex_search(linkName, match, jointRegex))
{
return false;
}
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggestion: Might want to consider StringFunc::StartsWith / EndsWith / Contains vs regex. Not super important here, but as we've seen in the scene code, regex can bring a hefty CPU cost.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Actually I'll change this to use StartsWith and EndsWithd for the wheelRegex.

Copy link
Contributor Author

@lemonade-dm lemonade-dm Aug 17, 2023

Choose a reason for hiding this comment

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

I also just realized the jointRegex comment was indicates that the name should contain the word "joint" within it.
https://github.com/o3de/o3de-extras/blame/21affbdc4fc585395478d8eb587e8a9ba4b0b5d1/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp#L104-L108

Since the name is for a link, I believe the comment should be that it should NOT contain the word "joint" @michalpelka ?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Also is there a need for the "joint" heuristic to filter out "wheel" links that contain the whose name also contained "joint"?

I imagine if the "wheel_" or "_wheel" heuristic above matched, then there is no need to check if the link name also contained the term "joint".

Especially since the element being checked is a strongly typed Link class in C++.

@lemonade-dm
Copy link
Contributor Author

lemonade-dm commented Aug 17, 2023

@mbalfour-amzn Concerning comment #460 (comment)

Well some reasons for keeping the SDF parsing logic separate from the URDF parsing logic, is that URDFs go through some transformations that SDFs don't go through on conversion.

  1. For example, URDF internal conversion to SDF merges links together that are bound by fixed joints.
    However loading a regular SDF does not do that.
    In order to work around, that we have to set specific ParserConfig setting of preserveFixedJoints when parsing a URDF file to maintain the current behavior

A lazier reason is that it would require renaming a bunch of the places that mention URDF to specify URDF/SDF as well as files and functions that contain URDF would need to be renamed to a more general term like Robot.

Added a "/O3DE/ROS2/SdfAssetBuilder/URDFPreserveFixedJoint" setting which is used to preserve fixed joints when parsing URDFs

In order to use the URDFPreserveFixedJoint setting, the UrdfParser `Parse*` functions have been updated to accept an sdf::ParserConfig which can be used to set the preserve fixed joint setting.

Refactored the RobotImporter.cpp ResolveURDFPath function to support "model://" URI prefix when importing through the Widget.

Fixed the GetAllLinks function population of the SDF LinkMap to not use a dangling reference to the Link Name.

Fixed building of ROS2 Gem when Unity builds are turned off.
The SdfAssetBuilderName was being used in the SdfAssetBuilderSettings.cpp but it was only declared in the SdfAssetBuilder.cpp file

Moved the function that converts a sdf::Error vector into an AZStd::string to a Common ErrorUtils.cpp/h file.

Signed-off-by: lumberyard-employee-dm <[email protected]>
@jhanca-robotecai
Copy link
Contributor

A lazier reason is that it would require renaming a bunch of the places that mention URDF to specify URDF/SDF as well as files and functions that contain URDF would need to be renamed to a more general term like Robot.

Renaming is a separate task for a separate PR, but it should stay somewhere on a todo list. It is getting more and more messy with methods in the URDF namespace getting SDFormat types as parameters.

Copy link
Contributor

@jhanca-robotecai jhanca-robotecai left a comment

Choose a reason for hiding this comment

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

  • nonunity build works correctly now
  • tests pass with no problems
  • links are detected correctly

I got segmentation faults with our testcase 2 due to nullptr, as detailed in comments.

createdLinks[rootName] = createEntityRoot;
if (!createEntityRoot.IsSuccess())
{
return AZ::Failure(AZStd::string(createEntityRoot.GetError()));
}

auto links = Utils::GetAllLinks(m_model->root_link_->child_links);
auto links = Utils::GetAllLinks(*GetFirstModel(), true);
Copy link
Contributor

@jhanca-robotecai jhanca-robotecai Aug 18, 2023

Choose a reason for hiding this comment

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

Please correct me if I am wrong, but the following code will cause a duplication of the first link (the first link is already created in lines 135-136

image

Edit: generated using manual test after the segfaults were fixed

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes, this needs to be fixed.

There is no singular "root" link in an SDF file as it represents a graph structure which is different from URDF which is a tree structure where links the link hierarchy is established via the parent/child references in joints.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

The root link parsing logic has been fixed.
image

When a fixed SDF joint is processed, it doesn't have a JointAxis object on it and therefore a nullptr check is need to prevent dereferencing it in the Articulations and Joints Maker classes.

Signed-off-by: lumberyard-employee-dm <[email protected]>
@jhanca-robotecai
Copy link
Contributor

I can confirm the link structure is correct now and the issue of nullptr axes is solved. The main problem for me is that libsdformat ignores links with no <inertia>, which crashes the import of some popular robots such as Universal Robots. I am not sure if this is a blocker (could we assume the robot model is incorrect?), but I would definitely prefer to see the outcome results somehow. Currently, we can see the following message in the O3DE console:

(ParseXacro) - xacro executable : xacro
(ParseXacro) - Convert xacro file : /home/jhanca/devtrunk/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro
(ParseXacro) - calling file : xacro /home/jhanca/devtrunk/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro ur_type:=ur5e name:=FooRobot
(ParseXacro) - xacro finished with success

which makes sense, as xacro is parsed correctly. However, the importer wizard widget shows the problem with no explanation:
Screenshot from 2023-08-21 12-47-40
Finally, the true reason for the failure can be read in the terminal used to start O3DE:

Warning [parser_urdf.cc:2774] Error Code 19: Msg: urdf2sdf: link[base_link] has no <inertial> block defined. Please ensure this link has a valid mass to prevent any missing links or joints in the resulting SDF model.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed parent joint[base_joint], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: parent joint[base_joint] ignored.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed child joint[base_link-base_fixed_joint], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed child joint[base_link-base_link_inertia], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: [2] child joints ignored.
Error Code 19: Msg: urdf2sdf: [2] child links ignored.
Warning [parser_urdf.cc:2777] Error Code 19: Msg: urdf2sdf: link[base_link] is not modeled in sdf.

Shouldn't this be anyhow displayed in the GUI?

@lemonade-dm
Copy link
Contributor Author

lemonade-dm commented Aug 21, 2023

I can confirm the link structure is correct now and the issue of nullptr axes is solved. The main problem for me is that libsdformat ignores links with no <inertia>, which crashes the import of some popular robots such as Universal Robots. I am not sure if this is a blocker (could we assume the robot model is incorrect?), but I would definitely prefer to see the outcome results somehow. Currently, we can see the following message in the O3DE console:

(ParseXacro) - xacro executable : xacro
(ParseXacro) - Convert xacro file : /home/jhanca/devtrunk/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro
(ParseXacro) - calling file : xacro /home/jhanca/devtrunk/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro ur_type:=ur5e name:=FooRobot
(ParseXacro) - xacro finished with success

which makes sense, as xacro is parsed correctly. However, the importer wizard widget shows the problem with no explanation: Screenshot from 2023-08-21 12-47-40 Finally, the true reason for the failure can be read in the terminal used to start O3DE:

Warning [parser_urdf.cc:2774] Error Code 19: Msg: urdf2sdf: link[base_link] has no <inertial> block defined. Please ensure this link has a valid mass to prevent any missing links or joints in the resulting SDF model.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed parent joint[base_joint], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: parent joint[base_joint] ignored.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed child joint[base_link-base_fixed_joint], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: allowing joint lumping by removing any <disableFixedJointLumping> or <preserveFixedJoint> gazebo tag on fixed child joint[base_link-base_link_inertia], as well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, could help resolve this warning. See http://sdformat.org/tutorials?tut=sdformat_urdf_extensions for more information about this behavior.
Error Code 19: Msg: urdf2sdf: [2] child joints ignored.
Error Code 19: Msg: urdf2sdf: [2] child links ignored.
Warning [parser_urdf.cc:2777] Error Code 19: Msg: urdf2sdf: link[base_link] is not modeled in sdf.

Shouldn't this be anyhow displayed in the GUI?

allowing joint lumping by

Looking at the parser_urdf.cc source, the errors are getting recorded to the nonJointReductionErrors sdf::Error vector and then output to the sdfWarn stream at the end of converting the URDF to SDF.

In order to capture that output we can follow @michalpelka suggestion of capturing the sdf::Console::ConsoleStream by using the Unit Test code as an example.

Now on the issue with SDFormat ignoring links with no inertia mass inside a URDF, there is logic that does allow the links to be used if fixed joint reduction is enabled.

We can try importing those URDF with the /O3DE/ROS2/SdfAssetBuilder/URDFPreserveFixedJoint setting that was added in this PR to false.

@lemonade-dm
Copy link
Contributor Author

ParseXacro

Actually it turns out the URDF parsing Errors with inertia are captured, but there is an existing bug in the RobotImporter UI where the xacro ExecutionOutcome check encompasses both that the xacro command succeeded and the URDF parsing command succeeded.

However the UI logic assumes that if only the xacro command succeeded, but not the URDF command, then it would output false.

URDFs with links without inertia that are not merged with other links that do have inertia via joint reduction causes the URDF to fail to parse using SDF.

When the `UrdfPreserveFixedJoint` setting is turned off, a URDF that doesn't have an inertial tag can be merged with a link that does have an inertial tag and allow the import to succeed.

Signed-off-by: lumberyard-employee-dm <[email protected]>
@lemonade-dm
Copy link
Contributor Author

@adamdbrw @jhanca-robotecai It turns out that using the URDFPreserveFixedJoint option would cause URDFs to fail to import if they have links without inertia that are part of fixed joints.

Unit Test have been added to verify that case in commit 7a8414d

As that setting is configurable via the Settings Registry, a user can workaround import issues by overriding the /O3DE/ROS2/SdfAssetBuilder/URDFPreserveFixedJoint value in their own .setreg file to false

@jhanca-robotecai
Copy link
Contributor

@adamdbrw @jhanca-robotecai It turns out that using the URDFPreserveFixedJoint option would cause URDFs to fail to import if they have links without inertia that are part of fixed joints.

Great to know, thanks for the debug! Now the question is if we want to keep this option set to true or false by default.

…he bounds are (-∞, ∞).

Updated the FilePath code to also mark files with the .world extension as SDF files.

Signed-off-by: lumberyard-employee-dm <[email protected]>
Copy link
Contributor

@jhanca-robotecai jhanca-robotecai left a comment

Choose a reason for hiding this comment

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

The code fails to correctly import UR5 robot

* thread #1, name = 'Editor', stop reason = signal SIGINT
  * frame #0: 0x00007ffff7096a7c libc.so.6`__GI___pthread_kill at pthread_kill.c:44:76
    frame #1: 0x00007ffff7096a30 libc.so.6`__GI___pthread_kill [inlined] __pthread_kill_internal(signo=2, threadid=140737343843584) at pthread_kill.c:78:10
    frame #2: 0x00007ffff7096a30 libc.so.6`__GI___pthread_kill(threadid=140737343843584, signo=2) at pthread_kill.c:89:10
    frame #3: 0x00007ffff7042476 libc.so.6`__GI_raise(sig=2) at raise.c:26:13
    frame #4: 0x000055555613bcd8 Editor`AZ::Debug::Platform::DebugBreak() at Trace_UnixLike.cpp:97:13
    frame #5: 0x000055555697525d Editor`AZ::Debug::Trace::Break(this=<unavailable>) at Trace.cpp:282:9
    frame #6: 0x0000555556975a60 Editor`AZ::Debug::Trace::Assert(this=0x00007fffffffda90, fileName=<unavailable>, line=<unavailable>, funcName=<unavailable>, format=<unavailable>) at Trace.cpp:0
    frame #7: 0x00007fff2fff3c30 libROS2.Editor.so`ROS2::URDFPrefabMaker::CreatePrefabTemplateFromURDF() [inlined] AZStd::unordered_map<AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator>, AZ::Outcome<AZ::EntityId, AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator> >, AZStd::hash<AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator> >, AZStd::equal_to<AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator> >, AZStd::allocator>::at(this=0x00007fffffffaa90, key=0x00007fffffffa9d0) at unordered_map.h:219:13
    frame #8: 0x00007fff2fff3be6 libROS2.Editor.so`ROS2::URDFPrefabMaker::CreatePrefabTemplateFromURDF(this=0x0000555573840010) at URDFPrefabMaker.cpp:272:44
    frame #9: 0x00007fff2fff60ed libROS2.Editor.so`ROS2::URDFPrefabMaker::CreatePrefabFromURDF(this=0x0000555573840010) at URDFPrefabMaker.cpp:351:23
    frame #10: 0x00007fff300371a0 libROS2.Editor.so`ROS2::RobotImporterWidget::CreatePrefab(this=0x0000555570304340, prefabName=AZStd::string @ 0x00007fffffffb5e8) at RobotImporterWidget.cpp:407:45
    frame #11: 0x00007fff30032e44 libROS2.Editor.so`ROS2::RobotImporterWidget::onCreateButtonPressed(this=0x0000555570304340) at RobotImporterWidget.cpp:426:9

Comment on lines 106 to 108
const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
|| jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity()
using LimitType = decltype(jointAxis->Upper());
Copy link
Contributor

Choose a reason for hiding this comment

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

Added semicolon; placed declaration before the first use

Suggested change
const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
|| jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity()
using LimitType = decltype(jointAxis->Upper());
using LimitType = decltype(jointAxis->Upper());
const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
|| jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity();

{
AZ_Assert(GetFirstModel(), "SDF Model is nullptr");

VisualsMaker::MaterialMap materialMap;
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
VisualsMaker::MaterialMap materialMap;
VisualsMaker::MaterialNameMap materialMap;

@@ -32,5 +32,7 @@ namespace ROS2

AZStd::vector<AssetBuilderSDK::AssetBuilderPattern> m_builderPatterns;
bool m_useArticulations = true;
// By default, fixed joint in URDF files that are processed by libsdformat are preserved
bool m_urdfPreserveFixedJoints = true;
};
} // ROS2
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
} // ROS2
} // namespace ROS2

@lemonade-dm
Copy link
Contributor Author

lemonade-dm commented Aug 22, 2023

The code fails to correctly import UR5 robot

* thread #1, name = 'Editor', stop reason = signal SIGINT
  * frame #0: 0x00007ffff7096a7c libc.so.6`__GI___pthread_kill at pthread_kill.c:44:76
    frame #1: 0x00007ffff7096a30 libc.so.6`__GI___pthread_kill [inlined] __pthread_kill_internal(signo=2, threadid=140737343843584) at pthread_kill.c:78:10
    frame #2: 0x00007ffff7096a30 libc.so.6`__GI___pthread_kill(threadid=140737343843584, signo=2) at pthread_kill.c:89:10
    frame #3: 0x00007ffff7042476 libc.so.6`__GI_raise(sig=2) at raise.c:26:13
    frame #4: 0x000055555613bcd8 Editor`AZ::Debug::Platform::DebugBreak() at Trace_UnixLike.cpp:97:13
    frame #5: 0x000055555697525d Editor`AZ::Debug::Trace::Break(this=<unavailable>) at Trace.cpp:282:9
    frame #6: 0x0000555556975a60 Editor`AZ::Debug::Trace::Assert(this=0x00007fffffffda90, fileName=<unavailable>, line=<unavailable>, funcName=<unavailable>, format=<unavailable>) at Trace.cpp:0
    frame #7: 0x00007fff2fff3c30 libROS2.Editor.so`ROS2::URDFPrefabMaker::CreatePrefabTemplateFromURDF() [inlined] AZStd::unordered_map<AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator>, AZ::Outcome<AZ::EntityId, AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator> >, AZStd::hash<AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator> >, AZStd::equal_to<AZStd::basic_string<char, AZStd::char_traits<char>, AZStd::allocator> >, AZStd::allocator>::at(this=0x00007fffffffaa90, key=0x00007fffffffa9d0) at unordered_map.h:219:13
    frame #8: 0x00007fff2fff3be6 libROS2.Editor.so`ROS2::URDFPrefabMaker::CreatePrefabTemplateFromURDF(this=0x0000555573840010) at URDFPrefabMaker.cpp:272:44
    frame #9: 0x00007fff2fff60ed libROS2.Editor.so`ROS2::URDFPrefabMaker::CreatePrefabFromURDF(this=0x0000555573840010) at URDFPrefabMaker.cpp:351:23
    frame #10: 0x00007fff300371a0 libROS2.Editor.so`ROS2::RobotImporterWidget::CreatePrefab(this=0x0000555570304340, prefabName=AZStd::string @ 0x00007fffffffb5e8) at RobotImporterWidget.cpp:407:45
    frame #11: 0x00007fff30032e44 libROS2.Editor.so`ROS2::RobotImporterWidget::onCreateButtonPressed(this=0x0000555570304340) at RobotImporterWidget.cpp:426:9

I am looking into the issue

@jhanca-robotecai
Copy link
Contributor

I an looking into the issue

I did check it as well. This robot cannot be imported when all links are preserved, because some links have no <inertial> description. On the other hand, when we allow links to be removed, it crashes (as above) when creating joints, because parents are missing. The crash above is triggered when parser tries to find world link, which does not exist, as urdf lookos as follows:

  <!-- create link fixed to the "world" -->
  <link name="world"/>

Signed-off-by: lumberyard-employee-dm <[email protected]>
If a joint references a parent or child link that doesn't exist, the code would assert in unordered_map

Signed-off-by: lumberyard-employee-dm <[email protected]>
@lemonade-dm
Copy link
Contributor Author

lemonade-dm commented Aug 24, 2023

I an looking into the issue

I did check it as well. This robot cannot be imported when all links are preserved, because some links have no <inertial> description. On the other hand, when we allow links to be removed, it crashes (as above) when creating joints, because parents are missing. The crash above is triggered when parser tries to find world link, which does not exist, as urdf lookos as follows:

  <!-- create link fixed to the "world" -->
  <link name="world"/>

There is actually special logic in the SDF joint reduction code that skips the first link name if its name is "world" https://github.com/gazebosim/sdformat/blob/main/src/parser_urdf.cc#L422-L424

The URDF parsing logic also skips creating the "world" link if it is a root link in the URDF.

However it doesn't remove the joint that contains the "world" link as a parent, so the converted SDF still has joint that references a parent "link" of world, but doesn't have a "link" with that name

What I have done is have the Joint Visitor logic skip any joints where its parent link reference is "world", but an actual "world" link doesn't exist in the converted SDF?

…exist via supplying the `--help` argument before attempting to run it with the input file.

Signed-off-by: lumberyard-employee-dm <[email protected]>
Joints which do not have a valid parent or child link will be skipped during visting using the `ROS2::Utils::VisitJoints` API.

Updated the URDF Prefab Makaer to use the VisitJoints API to only visit joints which have links that are part of the SDF.

With this change, URDFs that are parsed with a root link that has the name of "world" will now skip over visting joints where the parent link being referenced is the "world" link.

This is needed because the libsdformat URDF parser skips converstion of root links with the name of "world" when converting to SDF, therefore the joints which references the "world" link also needs to be skipped.

Signed-off-by: lumberyard-employee-dm <[email protected]>
Copy link
Contributor

@jhanca-robotecai jhanca-robotecai left a comment

Choose a reason for hiding this comment

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

All my concerns were fixed. There are robots that can be correctly imported:

We should also rethink the configuration of the URDFPreserveFixedJoint parameter. It is set to true by default, but maybe it should be set to false by default as used in the majority of cases. Moreover, a possibility to change it from the wizard would be an added value (but it depends if we want to keep the wizard alive or not).

There are also some robots, that fail to import, in particular panda franka and fanuc. I believe the problems should be investigated further, but this PR should be merged to unblock further work.

I suggest to create the following issues and continue the work from here:

  1. Investigate problems importing panda franka and fanuc robots with SDFormat library; double-check the warnings triggered by pr2 import
  2. Add SDFormat parsing output (warnings and errors) to the wizard.
  3. Add parameter change URDFPreserveFixedJoint to wizard

@lemonade-dm lemonade-dm merged commit b1fc926 into o3de:development Aug 25, 2023
@lemonade-dm
Copy link
Contributor Author

lemonade-dm commented Aug 25, 2023

All my concerns were fixed. There are robots that can be correctly imported:

* husky

* jackal

* mini_pupper (some known problems with assets [Reverted PR #400, fixes to URDF importer #469](https://github.com/o3de/o3de-extras/pull/469) )

* otto1500 (with some warnings)

* pr2 (with some warnings)

* robotis op3

* rosbot_xl

* universal robots ur10e and ur5e
  ![image](https://user-images.githubusercontent.com/134940295/263266025-c3da9342-2c2f-42ed-a004-c13a752a006a.png)

We should also rethink the configuration of the URDFPreserveFixedJoint parameter. It is set to true by default, but maybe it should be set to false by default as used in the majority of cases. Moreover, a possibility to change it from the wizard would be an added value (but it depends if we want to keep the wizard alive or not).

There are also some robots, that fail to import, in particular panda franka and fanuc. I believe the problems should be investigated further, but this PR should be merged to unblock further work.

I suggest to create the following issues and continue the work from here:

1. Investigate problems importing [panda franka and fanuc](https://github.com/ros-planning/moveit_resources) robots with SDFormat library; double-check the warnings triggered by  [pr2](https://github.com/ros-planning/moveit_resources) import

2. Add SDFormat parsing output (warnings and errors) to the wizard.

3. Add parameter change _URDFPreserveFixedJoint_ to wizard

I added a GHI for investigating the Move-it resources repo Robot Models: #472

For #2 we propagate the SDFormat errors that is returned by the parser API itself and output it to the Import dialog via markdown if the import fails (

log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());
).
However if the import succeeds, we don't output any warnings. I did make a shelf of capturi
ng the SDF console output when parsing occurs that can be introduced to capture messages even when parsing succeeds
console_capture.patch.

For number #3, we can add EditContext reflection to the SDFAssetBuilderSettings reflection and expose those to import wizard use the Reflected Property Editor system on their own page (example of creating a Property Editor using a class reflection is here)

@lemonade-dm lemonade-dm deleted the urdf-importer-refactor branch August 25, 2023 18:52
@lemonade-dm
Copy link
Contributor Author

All my concerns were fixed. There are robots that can be correctly imported:

* husky

* jackal

* mini_pupper (some known problems with assets [Reverted PR #400, fixes to URDF importer #469](https://github.com/o3de/o3de-extras/pull/469) )

* otto1500 (with some warnings)

* pr2 (with some warnings)

* robotis op3

* rosbot_xl

* universal robots ur10e and ur5e
  ![image](https://user-images.githubusercontent.com/134940295/263266025-c3da9342-2c2f-42ed-a004-c13a752a006a.png)

We should also rethink the configuration of the URDFPreserveFixedJoint parameter. It is set to true by default, but maybe it should be set to false by default as used in the majority of cases. Moreover, a possibility to change it from the wizard would be an added value (but it depends if we want to keep the wizard alive or not).
There are also some robots, that fail to import, in particular panda franka and fanuc. I believe the problems should be investigated further, but this PR should be merged to unblock further work.
I suggest to create the following issues and continue the work from here:

1. Investigate problems importing [panda franka and fanuc](https://github.com/ros-planning/moveit_resources) robots with SDFormat library; double-check the warnings triggered by  [pr2](https://github.com/ros-planning/moveit_resources) import

2. Add SDFormat parsing output (warnings and errors) to the wizard.

3. Add parameter change _URDFPreserveFixedJoint_ to wizard

For #2 we propagate the SDFormat errors that is returned by the parser API itself and output it to the Import dialog via markdown if the import fails (

log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());

).
However if the import succeeds, we don't output any warnings. I did make a shelf of capturi
ng the SDF console output when parsing occurs that can be introduced to capture messages even when parsing succeeds
console_capture.patch.

For number #3, we can add EditContext reflection to the SDFAssetBuilderSettings reflection and expose those to import wizard use the Reflected Property Editor system on their own page (example of creating a Property Editor using a class reflection is here)

I added a PR to capture the sdf Console messages as well as add the SDF AssetBuilder Settings to the Robot Importer: #471

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants