Parses custom LinearBushingRollPitchYaw tag in SDF/URDF #13255

joemasterjohn · 2020-05-07T16:00:28Z

WIP to parse a custom tag to specify LinearBushingRollPitchYaw force elements from an SDF or URDF file.

This change is

amcastro-tri

Reviewed 4 of 5 files at r1.
Reviewable status: 5 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @joemasterjohn)

multibody/parsing/detail_common.h, line 63 at r1 (raw file):

/// controlled/modified in a single function.

/// SDF:

you might want to add a lil header with __SDF__ (double underscores, or even <h4>

multibody/parsing/detail_common.h, line 86 at r1 (raw file):

///   <drake:bushing_force_stiffness  value="0 0 0"/>
///   <drake:bushing_force_damping    value="0 0 0"/>
/// </drake:linear_bushing_rpy>

btw, I love this SDF/URDF syntax and how homogeneous you kept it across formats.

multibody/parsing/detail_common.h, line 89 at r1 (raw file):

///

const LinearBushingRollPitchYaw<double>& ParseLinearBushingRollPitchYaw(

nit, look out for empty spaces, new lines etc.

multibody/parsing/detail_sdf_parser.cc, line 591 at r1 (raw file):

    }

    return plant->GetFrameByName(frame_name);

you might want to check the frame actually exists with MBP::HasFrameNamed()

multibody/parsing/detail_sdf_parser.cc, line 676 at r1 (raw file):

  // This is super inefficient because each call to `HasElement` and
  // `NextElement` loop through all of the the models child elements, first to
  // find the current node, and then to go from that iterator to the next

is it inefficient? why? this is because you looked at the implementation?
I mean, the API calls look good. So I imagine if inneficient just because the underlying implementation is bad. So what would be the action item or TODO here? is there anything this code should modify? or more like an issue to be opened in sdformat?
That is, if sdformat fixes something, would this code change? because in that case this comment would go out of date and most likely no one will notice.

multibody/parsing/detail_urdf_parser.cc, line 581 at r1 (raw file):

    }

    return plant->GetFrameByName(name);

ditto, double check frame exists.

amcastro-tri

Reviewed 1 of 5 files at r1.
Reviewable status: 5 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @joemasterjohn)

joemasterjohn

+@amcastro-tri for feature review, please!

Reviewable status: 4 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

multibody/parsing/detail_common.h, line 63 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

you might want to add a lil header with __SDF__ (double underscores, or even <h4>

Done.

multibody/parsing/detail_common.h, line 86 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

btw, I love this SDF/URDF syntax and how homogeneous you kept it across formats.

Thanks!

multibody/parsing/detail_sdf_parser.cc, line 591 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

you might want to check the frame actually exists with MBP::HasFrameNamed()

Done.

multibody/parsing/detail_sdf_parser.cc, line 676 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

is it inefficient? why? this is because you looked at the implementation?
I mean, the API calls look good. So I imagine if inneficient just because the underlying implementation is bad. So what would be the action item or TODO here? is there anything this code should modify? or more like an issue to be opened in sdformat?
That is, if sdformat fixes something, would this code change? because in that case this comment would go out of date and most likely no one will notice.

I took a look at the implementation in libSDF. This was more a reminder to myself to find a more efficient API if one existed, or maybe the reviewer might know one. I have no proof that it is a burden on runtime or anything, so I think that I will just remove this comment.

multibody/parsing/detail_urdf_parser.cc, line 581 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto, double check frame exists.

Done.

amcastro-tri

Thanks @joemasterjohn. Feature review done,

+@soonho-tri for platform review please.

Reviewed 5 of 5 files at r2.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee soonho-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn and @soonho-tri)

multibody/parsing/detail_sdf_parser.cc, line 676 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

I took a look at the implementation in libSDF. This was more a reminder to myself to find a more efficient API if one existed, or maybe the reviewer might know one. I have no proof that it is a burden on runtime or anything, so I think that I will just remove this comment.

ok. I still don't understand how it could be better. I don't understand why it'd be inneficient (not without looking at sdformat's code).
So yes, if not anymore relevant, remove.

multibody/parsing/detail_sdf_parser.cc, line 566 at r2 (raw file):

    if (!successful) {
      throw std::runtime_error(fmt::format(
          "Unable to read the value of the <{}> tag.", element_name));

nit, when does this throw? for instance if I do torque_stiffness = [1.0 2.0 b]?

multibody/parsing/detail_sdf_parser.cc, line 585 at r2 (raw file):

    auto [frame_name, successful] =
        node->Get<std::string>(element_name, std::string());

nit, could we document:?

node->Get<std::string>(element_name, std::string() /* default value. not used */);

multibody/parsing/detail_urdf_parser.cc, line 554 at r2 (raw file):

    const XMLElement* value_node = node->FirstChildElement(element_name);
    if (value_node != nullptr) {
      Eigen::Vector3d value{};

fyi, not sure a defect but, why the empty initializer list?

multibody/parsing/detail_urdf_parser.cc, line 560 at r2 (raw file):

        throw std::runtime_error(
            fmt::format("Unable to read the 'value' attribute for the <{}> "
                        "tag on line {}",

nit, when does this throw?

multibody/parsing/detail_urdf_parser.cc, line 590 at r2 (raw file):

      } else {
        throw std::runtime_error(
            fmt::format("Unable to read the 'name' attribute for the <{}> "

nit, when does this throw?

multibody/parsing/test/detail_sdf_parser_test.cc, line 769 at r2 (raw file):

  // Test successful parsing
  auto [plant, scene_graph] = ParseTestString(R"(

nit, remove blank line?

multibody/parsing/test/detail_sdf_parser_test.cc, line 809 at r2 (raw file):

      <drake:linear_bushing_rpy>
        <drake:bushing_frameA>frameA</drake:bushing_frameA>

nit, instead of blank line, maybe add comment like 

multibody/parsing/test/detail_sdf_parser_test.cc, line 816 at r2 (raw file):

      </drake:linear_bushing_rpy>
    </model>)"),
                              std::runtime_error,

nit, is this indentation ok? at least not consistent with the one below

multibody/parsing/test/detail_urdf_parser_test.cc, line 411 at r2 (raw file):

 
        <drake:linear_bushing_rpy>
            <drake:bushing_frameA name="frameA"/>

nit, trailing spaces show up as lil red dots in Reviewable

multibody/parsing/test/detail_urdf_parser_test.cc, line 429 at r2 (raw file):

        <frame name="frameA" link="A" rpy="0 0 0" xyz="0 0 0"/>
        <frame name="frameC" link="C" rpy="0 0 0" xyz="0 0 0"/>

nit, more red dots.

joemasterjohn

Reviewable status: 3 unresolved discussions, LGTM missing from assignee soonho-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @soonho-tri)

multibody/parsing/detail_sdf_parser.cc, line 676 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ok. I still don't understand how it could be better. I don't understand why it'd be inneficient (not without looking at sdformat's code).
So yes, if not anymore relevant, remove.

Ok removed. GetNextElement is O(n) making the whole loop O(n²). If I had access to the collection storing the elements I could make the loop O(n), but GetElement and GetNextElement are the only public exposed APIs.

multibody/parsing/detail_sdf_parser.cc, line 566 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, when does this throw? for instance if I do torque_stiffness = [1.0 2.0 b]?

After reading the API more carefully, I don't think this will throw. The API specifies that the second returned value (successful) will be false if it cannot find the first argument (element_name).

It seems like libsdformat does undefined things if you have malformed data in the tag. For instance:
<tag></tag> parses to 0 0 0
<tag>1 2 b</tag> parses to 1 2 0
<tag>b 2 3</tag> parses to 0 0 1.02766e-321 (and other junk on differently primed runs)

Either way it doesn't throw any errors so I don't seem to have a way to detect bad input here.

multibody/parsing/detail_urdf_parser.cc, line 554 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

fyi, not sure a defect but, why the empty initializer list?

No reason, removed.

multibody/parsing/detail_urdf_parser.cc, line 560 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, when does this throw?

When the attribute doesn't exist in the tag. For instance <drake:bushing_torque_stiffness /> would trigger. I realized I don't have test coverage for that. Adding now.

multibody/parsing/detail_urdf_parser.cc, line 590 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, when does this throw?

Same as above comment for read_vector.

amcastro-tri

Reviewed 4 of 4 files at r3.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee soonho-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn and @soonho-tri)

multibody/parsing/detail_sdf_parser.cc, line 676 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Ok removed. GetNextElement is O(n) making the whole loop O(n²). If I had access to the collection storing the elements I could make the loop O(n), but GetElement and GetNextElement are the only public exposed APIs.

O(n)? but why? that's awful. Did you look at the source?

multibody/parsing/detail_sdf_parser.cc, line 566 at r2 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

After reading the API more carefully, I don't think this will throw. The API specifies that the second returned value (successful) will be false if it cannot find the first argument (element_name).

It seems like libsdformat does undefined things if you have malformed data in the tag. For instance:
<tag></tag> parses to 0 0 0
<tag>1 2 b</tag> parses to 1 2 0
<tag>b 2 3</tag> parses to 0 0 1.02766e-321 (and other junk on differently primed runs)

Either way it doesn't throw any errors so I don't seem to have a way to detect bad input here.

agh! that's horrible! could we open an issue in sdformat? and reference here if we do (there might be one opened already)

multibody/parsing/test/detail_urdf_parser_test.cc, line 376 at r3 (raw file):

        <frame name="frameA" link="A" rpy="0 0 0" xyz="0 0 0"/>
        <frame name="frameC" link="C" rpy="0 0 0" xyz="0 0 0"/>

nit, another red dot.

multibody/parsing/test/detail_urdf_parser_test.cc, line 409 at r3 (raw file):

        <frame name="frameA" link="A" rpy="0 0 0" xyz="0 0 0"/>
        <frame name="frameC" link="C" rpy="0 0 0" xyz="0 0 0"/>

nit, another red dot. more below.

jwnimmer-tri · 2020-05-13T17:56:23Z

multibody/parsing/detail_sdf_parser.cc, line 566 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

agh! that's horrible! could we open an issue in sdformat? and reference here if we do (there might be one opened already)

FYI See gazebosim/sdformat#228 and gazebosim/sdformat#260

SeanCurtis-TRI

Reviewable status: 3 unresolved discussions, LGTM missing from assignee soonho-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @joemasterjohn, @jwnimmer-tri, and @soonho-tri)

multibody/parsing/detail_sdf_parser.cc, line 566 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

FYI See gazebosim/sdformat#228 and gazebosim/sdformat#260

I encountered the same issue with parsing colors. This is part of the reason I don't like it when parsers provide default values.

soonho-tri

Reviewed 2 of 5 files at r1, 1 of 5 files at r2, 4 of 4 files at r3.
Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @joemasterjohn, and @jwnimmer-tri)

joemasterjohn

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @jwnimmer-tri, @SeanCurtis-TRI, and @soonho-tri)

multibody/parsing/detail_sdf_parser.cc, line 676 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

O(n)? but why? that's awful. Did you look at the source?

Yes, I read the source of Element::GetNextElement in Element.cc:624:
http://osrf-distributions.s3.amazonaws.com/sdformat/api/9.0.0/classsdf_1_1v9_1_1Element.html#a1e66a6def709f4cf49d55aa77db69787

In the doc they recommend a loop in the style I have here to walk the tree.

The element std::find's itself in its parent's std::vector of children, getting an iterator to itself. It then increments the iterator until it reaches the next node with the specified name.

multibody/parsing/detail_sdf_parser.cc, line 566 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I encountered the same issue with parsing colors. This is part of the reason I don't like it when parsers provide default values.

Thanks @jwnimmer-tri for having those issues open already so I didn't have to :)

joemasterjohn added 2 commits May 7, 2020 11:50

parsing of bushing tag

e4dbe2f

formatting

c58347a

amcastro-tri changed the title ~~Parses custom LinearBushingRollPitchYaw tag in SDF/URDF~~ [WIP] Parses custom LinearBushingRollPitchYaw tag in SDF/URDF May 7, 2020

amcastro-tri added status: do not merge status: do not review labels May 7, 2020

fixed missing include and formatting for linter

187e5b2

amcastro-tri reviewed May 7, 2020

View reviewed changes

added tests

286ff32

joemasterjohn marked this pull request as ready for review May 11, 2020 15:07

joemasterjohn removed status: do not merge status: do not review labels May 11, 2020

joemasterjohn assigned amcastro-tri May 11, 2020

joemasterjohn commented May 11, 2020

View reviewed changes

joemasterjohn changed the title ~~[WIP] Parses custom LinearBushingRollPitchYaw tag in SDF/URDF~~ Parses custom LinearBushingRollPitchYaw tag in SDF/URDF May 11, 2020

amcastro-tri added component: multibody plant MultibodyPlant and supporting code unused team: dynamics type: feature request labels May 11, 2020

amcastro-tri assigned soonho-tri May 13, 2020

amcastro-tri reviewed May 13, 2020

View reviewed changes

comments from review

9580356

joemasterjohn commented May 13, 2020

View reviewed changes

amcastro-tri reviewed May 13, 2020

View reviewed changes

SeanCurtis-TRI reviewed May 13, 2020

View reviewed changes

soonho-tri reviewed May 13, 2020

View reviewed changes

whitespace changes

3792721

joemasterjohn commented May 13, 2020

View reviewed changes

joemasterjohn added the status: squashing now https://drake.mit.edu/reviewable.html#curated-commits label May 13, 2020

joemasterjohn merged commit 574cafd into RobotLocomotion:master May 13, 2020

joemasterjohn mentioned this pull request May 13, 2020

Strandbeest example #13302

Merged

joemasterjohn mentioned this pull request May 27, 2020

Need parsing support for the bushing force element #13021

Closed

joemasterjohn deleted the bushing_parser branch June 16, 2020 18:26

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Parses custom LinearBushingRollPitchYaw tag in SDF/URDF #13255

Parses custom LinearBushingRollPitchYaw tag in SDF/URDF #13255

joemasterjohn commented May 7, 2020 •

edited by sherm1

Loading

amcastro-tri left a comment

amcastro-tri left a comment

joemasterjohn left a comment

amcastro-tri left a comment

joemasterjohn left a comment

amcastro-tri left a comment

jwnimmer-tri commented May 13, 2020

SeanCurtis-TRI left a comment

soonho-tri left a comment

joemasterjohn left a comment

Parses custom LinearBushingRollPitchYaw tag in SDF/URDF #13255

Parses custom LinearBushingRollPitchYaw tag in SDF/URDF #13255

Conversation

joemasterjohn commented May 7, 2020 • edited by sherm1 Loading

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

joemasterjohn left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

joemasterjohn left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

jwnimmer-tri commented May 13, 2020

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

soonho-tri left a comment

Choose a reason for hiding this comment

joemasterjohn left a comment

Choose a reason for hiding this comment

joemasterjohn commented May 7, 2020 •

edited by sherm1

Loading