-
Notifications
You must be signed in to change notification settings - Fork 65
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
Updating ROS2 RobotImporter to use libsdformat #460
Conversation
Signed-off-by: Mike Balfour <[email protected]>
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]>
f8ce870
to
5b125f2
Compare
There was a problem hiding this 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.
There was a problem hiding this 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() |
There was a problem hiding this comment.
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").
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
Unfortunately this contribution gives unexpected, and different results on our manual test. |
There was a problem hiding this 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", |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
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) | ||
{ |
There was a problem hiding this comment.
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).
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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." |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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." |
There was a problem hiding this comment.
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. | ||
*/ |
There was a problem hiding this comment.
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'; | ||
} |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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:
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
There was a problem hiding this 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
There are also some problems with importing meshes, e.g. UR5e
@@ -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)"); |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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?
gz::math::Pose3d resolvedPose; | ||
|
||
if (sdf::Errors poseResolveErrors = linkSemanticPos.Resolve(resolvedPose); | ||
!poseResolveErrors.empty()) |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
Outdated
Show resolved
Hide resolved
Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
Outdated
Show resolved
Hide resolved
Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
Outdated
Show resolved
Hide resolved
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.
|
I'll fix this on the side. |
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 |
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)"); |
There was a problem hiding this comment.
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) | ||
{ |
There was a problem hiding this comment.
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/ArticulationsMaker.cpp
Outdated
Show resolved
Hide resolved
// 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; | ||
} |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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 ?
There was a problem hiding this comment.
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++.
Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
Outdated
Show resolved
Hide resolved
@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.
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]>
Signed-off-by: lumberyard-employee-dm <[email protected]>
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. |
There was a problem hiding this 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.
Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp
Outdated
Show resolved
Hide resolved
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); |
There was a problem hiding this comment.
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
Edit: generated using manual test after the segfault
s were fixed
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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]>
I can confirm the link structure is correct now and the issue of
which makes sense, as xacro is parsed correctly. However, the importer wizard widget shows the problem with no explanation:
Shouldn't this be anyhow displayed in the GUI? |
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 |
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. |
…parsing fails Signed-off-by: lumberyard-employee-dm <[email protected]>
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]>
@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 |
Great to know, thanks for the debug! Now the question is if we want to keep this option set to |
Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp
Outdated
Show resolved
Hide resolved
…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]>
There was a problem hiding this 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
const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity() | ||
|| jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity() | ||
using LimitType = decltype(jointAxis->Upper()); |
There was a problem hiding this comment.
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
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; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
} // ROS2 | |
} // namespace ROS2 |
I am 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
|
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]>
…tyId Signed-off-by: lumberyard-employee-dm <[email protected]>
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]>
There was a problem hiding this 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:
- husky
- jackal
- mini_pupper (some known problems with assets Reverted PR #400, fixes to URDF importer #469 )
- otto1500 (with some warnings)
- pr2 (with some warnings)
- robotis op3
- rosbot_xl
- universal robots ur10e and ur5e
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:
- Investigate problems importing panda franka and fanuc robots with SDFormat library; double-check the warnings triggered by pr2 import
- Add SDFormat parsing output (warnings and errors) to the wizard.
- 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 (
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 |
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.