parsing: Fix joint pose computation when using frames as joint parent or child

[azeey]

This fixes the case where //joint/parent or //joint/child is a frame instead of a link and that frame has a non-identity pose. The poses of the frames were being ignored because we incorrectly computing X_PJ and X_CJ. Instead of resolving these poses relative to the parent or child bodies, we were resolving them relative to the names in //joint/parent or //joint/child, which may reference frames instead of bodies.

/cc @EricCousineau-TRI

---

This change is 

[EricCousineau-TRI]

First pass done, PTAL!

Reviewed 3 of 3 files at r1.
Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @azeey)

---

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

}

// Calculates the relative name of a body relative to the model instance @p

nit This sentence is hard to read given the number of times relative appears.

Is there a possibly clearer rewording? Or replace relative with scoped or unscoped?

---

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

    const std::string& nested_model_absolute_name =
        plant.GetModelInstanceName(body.model_instance());
    // The relative_to_model_absolute_name must be a prefix of the

nit This code is a bit hard to read; can you use something like StartsWith to make the code clearer (and thus need fewer comments)?

// Returns true if `str` starts with `prefix`.
bool StartsWith(const std::string_view str, const std::string_view prefix) {
  return prefix.size() > str.size() && std::equal(str.begin(), str.begin() + prefix.size(), prefix.begin());
}

...
const std::string required_prefix = relative_to_model_instance + kDelimiter;
DRAKE_DEMAND(StartsWith(nested_model_absolute_name, required_prefix));

---

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

    // nested_model_absolute_name. Otherwise the nested model is not a
    // descendent of the model relative to which we are computing the name.
    DRAKE_DEMAND(0 == nested_model_absolute_name.compare(

BTW In Drake, we tend to use DRAKE_DEMAND(expr == literal), not (literal == expr).

---

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

    MultibodyPlant<double>* plant,
    std::set<sdf::JointType>* joint_types) {
  const Body<double>& parent_body = GetBodyByLinkSpecificationName(

Is there any way to use parent_frame and child_frame instead of parent_body and child_body?

If so, that could remove the need to do string munging or additional math.
(given the number of comments above, would be ideal to do so, if it is indeed possible?)

[azeey]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI)

---

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

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit This sentence is hard to read given the number of times relative appears.

Is there a possibly clearer rewording? Or replace relative with scoped or unscoped?

Ok, replaced two instances of relative with scoped. Feels clearer to me.

---

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

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit This code is a bit hard to read; can you use something like StartsWith to make the code clearer (and thus need fewer comments)?

// Returns true if `str` starts with `prefix`.
bool StartsWith(const std::string_view str, const std::string_view prefix) {
  return prefix.size() > str.size() && std::equal(str.begin(), str.begin() + prefix.size(), prefix.begin());
}

...
const std::string required_prefix = relative_to_model_instance + kDelimiter;
DRAKE_DEMAND(StartsWith(nested_model_absolute_name, required_prefix));

Done. Thanks for the tip. Much cleaner with StartsWith.

---

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

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Is there any way to use parent_frame and child_frame instead of parent_body and child_body?

If so, that could remove the need to do string munging or additional math.
(given the number of comments above, would be ideal to do so, if it is indeed possible?)

Multibodyplant::AddJoint takes Body<T>s. I guess it would be feasible to create the joint here and call the Multibodyplant::AddJoint overload that takes std::unique_ptr<JointType<T>>. Is that what you have in mind?

[EricCousineau-TRI]

Reviewed 1 of 2 files at r2.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)

---

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

Previously, azeey (Addisu Z. Taddese) wrote…

Multibodyplant::AddJoint takes Body<T>s. I guess it would be feasible to create the joint here and call the Multibodyplant::AddJoint overload that takes std::unique_ptr<JointType<T>>. Is that what you have in mind?

Yup! The only caveat being the need for AddFrame<> if an offset is necessary.

[azeey]

---

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

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Yup! The only caveat being the need for AddFrame<> if an offset is necessary.

I tried to use parent_frame and child_frame, but it ends up needing a bunch of extra logic to find the frame in the MultibodyPlant. This is because
if the parent or child is a frame defined as a sibling of the joint, but attached to a link in a nested model, the Drake frame will be associated with the model instance of the nested model. For example, given the following SDFormat input, finding the child frame F1 of joint J1 will need extra logic its Drake frame is associated with the model instance of M1 instead of parent_model.

<model name='parent_model'>
  <link name='L1' />
  <model name='M1'>
    <link name='L2' />
  </model>
  <frame name='F1' attached_to='M1::L2' /> <!-- F1 is associated with the model instance of M1-->
  <joint name='J1' type='fixed'>
    <parent>L1</parent>
    <child>F1</child> 
  </joint>
</model>

Similarly, if the parent or child of the joint is the implicit frame of a joint or model, we need more logic to find the associated Drake frame because we can't simply look them up by name. So I'm inclined to keep using parent_body and child_body.

[EricCousineau-TRI]

with some minor nits - thanks!

+@sammy-tri for platform review, please! (not per schedule, but just relevance; let me know if you'd like me to reassign!)

Reviewed 1 of 2 files at r2.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform) (waiting on @azeey and @sammy-tri)

---

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

Previously, azeey (Addisu Z. Taddese) wrote…

I tried to use parent_frame and child_frame, but it ends up needing a bunch of extra logic to find the frame in the MultibodyPlant. This is because
if the parent or child is a frame defined as a sibling of the joint, but attached to a link in a nested model, the Drake frame will be associated with the model instance of the nested model. For example, given the following SDFormat input, finding the child frame F1 of joint J1 will need extra logic its Drake frame is associated with the model instance of M1 instead of parent_model.

<model name='parent_model'>
  <link name='L1' />
  <model name='M1'>
    <link name='L2' />
  </model>
  <frame name='F1' attached_to='M1::L2' /> <!-- F1 is associated with the model instance of M1-->
  <joint name='J1' type='fixed'>
    <parent>L1</parent>
    <child>F1</child> 
  </joint>
</model>

Similarly, if the parent or child of the joint is the implicit frame of a joint or model, we need more logic to find the associated Drake frame because we can't simply look them up by name. So I'm inclined to keep using parent_body and child_body.

OK Gotcha, thanks for checking!

At some point, this may motivate slightly different API and behavior, perhaps related to #15280; I think something like that may admit some simpler logic, but no need to hold this up on that account!

\cc @sammy-tri

---

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

    // J1p and J1c are the frames of J1 on its parent and child links
    // respectively. The absolute poses of both frames should be identical.
    const RigidTransformd X_WJ1_expected(RollPitchYawd(0, 0, 0),

nit Unsure why you're computing w.r.t. the world.

While I'm sure the values are designed to match up (e.g. given that the link origins are coincident with the world), I think the more logical test would be to compute X_PJp and X_CJc?

To compute, you can use either plant.CalcRelativeTransform(), or frame.CalcPose(context, frame_M).

---

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

                                          Vector3d(4, 5, 6));

    const RigidTransformd X_WJ1p =

BTW Given the scope of this sub-test, you could name these frames X_WJp and X_WJc

---

multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf, line 3 at r2 (raw file):

<?xml version="1.0"?>
<sdf version="1.8">
  <model name='parent_model'>

nit xmllint on this file uses double quotes "", rather than single quotes ''.

Can you change this file to use double quotes?
To see:

a=multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf
b=/tmp/format.xml
xmllint ${a} > ${b}
git diff --no-index ${a} ${b}

[EricCousineau-TRI]

@drake-jenkins-bot retest this please

Prior failures seem like a flake:
https://drake-jenkins.csail.mit.edu/job/linux-bionic-gcc-bazel-experimental-everything-release/9682/consoleFull
https://gist.github.com/EricCousineau-TRI/cea81a7ce4f84b489d7875c7ec478300

[sammy-tri]

No additional objections from me, waiting on nits to be resolved for final approval/merge.

Reviewed 1 of 3 files at r1, 2 of 2 files at r2.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform) (waiting on @azeey)

[azeey]

Reviewable status: LGTM missing from assignee sammy-tri(platform) (waiting on @EricCousineau-TRI and @sammy-tri)

---

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

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Unsure why you're computing w.r.t. the world.

While I'm sure the values are designed to match up (e.g. given that the link origins are coincident with the world), I think the more logical test would be to compute X_PJp and X_CJc?

To compute, you can use either plant.CalcRelativeTransform(), or frame.CalcPose(context, frame_M).

Done. I agree that's more logical. Changed to X_PJp and X_CJc.

---

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

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW Given the scope of this sub-test, you could name these frames X_WJp and X_WJc

Ok, since I changed the tests to use X_PJp and X_CJc, these aren't relevant anymore.

---

multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf, line 3 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit xmllint on this file uses double quotes "", rather than single quotes ''.

Can you change this file to use double quotes?
To see:

a=multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf
b=/tmp/format.xml
xmllint ${a} > ${b}
git diff --no-index ${a} ${b}

Done.

[EricCousineau-TRI]

Thanks! One add'l minor nit, but overall good!

Reviewed 3 of 3 files at r3.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform) (waiting on @azeey)

---

multibody/parsing/test/detail_sdf_parser_test.cc, line 1883 at r3 (raw file):

  const RigidTransformd X_CJc_expected = RigidTransformd::Identity();

  // In SDFormat Jp and Jc are coincident, so X_PJp = X_PJc, but since X_CJc is

nit Not sure what I gained from this comment since this code doesn't seem to test / explicitly rely on the value of X_PC- consider removing comment and placing the two lines together?

[EricCousineau-TRI]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform) (waiting on @azeey)

---

multibody/parsing/test/detail_sdf_parser_test.cc, line 1883 at r3 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Not sure what I gained from this comment since this code doesn't seem to test / explicitly rely on the value of X_PC- consider removing comment and placing the two lines together?

Ah, I see - but in Drake too, Jp and Jc are coincident at the zero configuration, so still not sure the value of comment?

[azeey]

---

multibody/parsing/test/detail_sdf_parser_test.cc, line 1883 at r3 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Ah, I see - but in Drake too, Jp and Jc are coincident at the zero configuration, so still not sure the value of comment?

Ok, removed. My intention was to describe how the value X_PJp_expected was computed because it's not a value you see in the .sdf file. If it's clear without the comment, then I'm okay with removing it.

[sammy-tri]

@drake-jenkins-bot linux-bionic-clang-bazel-experimental-release please

[sammy-tri]

CI looks like it was probably a flake.

Reviewed 2 of 3 files at r3, 1 of 1 files at r4.
Reviewable status: complete! all discussions resolved, LGTM from assignees sammy-tri(platform),EricCousineau-TRI(platform) (waiting on @azeey)