Add support for //include/placement_frame and //model/@placement_frame

[azeey]

Note that this implementation adds a <model name>::__placement_frame__ to included nested models as a workaround for the loss of the //model/@placement_frame attribute during parsing of such models. This can be removed once #283 lands.

Closes #319

---

This change is 

[codecov-commenter]

Codecov Report

Merging #324 into master will decrease coverage by 0.00%.
The diff coverage is 86.76%.

@@            Coverage Diff             @@
##           master     #324      +/-   ##
==========================================
- Coverage   87.60%   87.59%   -0.01%     
==========================================
  Files          60       60              
  Lines        9147     9215      +68     
==========================================
+ Hits         8013     8072      +59     
- Misses       1134     1143       +9     

Impacted Files	Coverage Δ
include/sdf/Error.hh	100.00% <ø> (ø)
src/FrameSemantics.cc	78.35% <76.19%> (-0.08%)	⬇️
src/Model.cc	88.43% <88.23%> (-0.03%)	⬇️
src/parser.cc	78.86% <100.00%> (+0.29%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 1ee1d50...9959445. Read the comment docs.

[EricCousineau-TRI]

Reviewable status: 0 of 11 files reviewed, 1 unresolved discussion (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

a discussion (no related file):
Should add TODO / issue to track nested model stuff as a follow-up, or implemented nested model tests in this PR once #316 is merged.

---

[scpeters]

@osrf-jenkins run tests please

[scpeters]

Reviewed 1 of 11 files at r1.
Reviewable status: 1 of 11 files reviewed, 4 unresolved discussions (waiting on @azeey and @scpeters)

a discussion (no related file):
I'd like to see tests that use //model/@placement_frame and nested //model/model/@placement_frame attributes directly without any //include elements, along with a test/sdf/invalid_placement_frame.sdf

---

---

sdf/1.8/model.sdf, line 18 at r1 (raw file):

If this element is specified, the pose must be specified.

why are we requiring this? I would expect to treat a missing pose element as if it were an identity pose. I'm not sure why it's required. (This applies to the other additions to the spec as well.)

---

src/Model.cc, line 403 at r1 (raw file):

direclty

directly

[azeey]

Reviewable status: 1 of 11 files reviewed, 4 unresolved discussions (waiting on @azeey and @scpeters)

---

sdf/1.8/model.sdf, line 18 at r1 (raw file):

Previously, scpeters (Steve Peters) wrote…

If this element is specified, the pose must be specified.

why are we requiring this? I would expect to treat a missing pose element as if it were an identity pose. I'm not sure why it's required. (This applies to the other additions to the spec as well.)

For //include/placement_frame, I think we should require it because a missing pose there is not identity pose. Instead, the pose of the included model is used without being overridden. For //model/@placement_frame, I think it would be safe to not require a pose.

[azeey]

Reviewable status: 1 of 16 files reviewed, 4 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

a discussion (no related file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Should add TODO / issue to track nested model stuff as a follow-up, or implemented nested model tests in this PR once #316 is merged.

Added a TODO in placement_frame test

---

a discussion (no related file):

Previously, scpeters (Steve Peters) wrote…

I'd like to see tests that use //model/@placement_frame and nested //model/model/@placement_frame attributes directly without any //include elements, along with a test/sdf/invalid_placement_frame.sdf

Added tests for //model/@placement_frame and invalid uses of placement_frame. I left a TODO for //model/model/@placement_frame since that requires #316.

---

---

src/Model.cc, line 403 at r1 (raw file):

Previously, scpeters (Steve Peters) wrote…

direclty

directly

Done. 3f0a8e8

[azeey]

Reviewable status: 1 of 16 files reviewed, 4 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

---

sdf/1.8/model.sdf, line 18 at r1 (raw file):

Previously, azeey (Addisu Taddese) wrote…

For //include/placement_frame, I think we should require it because a missing pose there is not identity pose. Instead, the pose of the included model is used without being overridden. For //model/@placement_frame, I think it would be safe to not require a pose.

Removed the requirement for //model/@placement_frame in b684dc1

[scpeters]

Reviewed 4 of 11 files at r1, 9 of 11 files at r2.
Reviewable status: 14 of 16 files reviewed, 6 unresolved discussions (waiting on @azeey and @EricCousineau-TRI)

a discussion (no related file):

Previously, azeey (Addisu Taddese) wrote…

Added tests for //model/@placement_frame and invalid uses of placement_frame. I left a TODO for //model/model/@placement_frame since that requires #316.

ok 👍

---

---

sdf/1.8/model.sdf, line 18 at r1 (raw file):

Previously, azeey (Addisu Taddese) wrote…

Removed the requirement for //model/@placement_frame in b684dc1

ok, sounds great

---

sdf/1.8/model.sdf, line 18 at r2 (raw file):

  </attribute>
  <attribute name="placement_frame" type="string" default="" required="0">
    <description>The frame inside this model whose pose will be set by the pose element of the model. i.e, the pose element specifies the pose of this frame instead of the model frame</description>

nit: add a . at the end of the sentence

---

src/Model.cc, line 422 at r2 (raw file):

        // We assume that all model frames are specified relative to their
        // parent frame, so using the RawPose should be okay.
        const auto &placementFramePose = childModelFrame->RawPose();

that works for now, but it will change when nested model support is merged

for example:
https://github.com/osrf/sdformat/pull/316/files#diff-a2d1f1ae2599df4eafdd728704bf7fa6

can you add a TODO about that?

---

src/parser.cc, line 1041 at r2 (raw file):

spacifying

specifying

---

src/parser.cc, line 1275 at r2 (raw file):

direclty

one more

---

test/integration/nested_model.cc, line 886 at r2 (raw file):

    }

    return nullptr;

there's a windows compiler warning saying this line is unreachable

https://build.osrfoundation.org/job/sdformat-ci-pr_any-windows7-amd64/1494/msbuild/new/

[azeey]

Reviewable status: 14 of 16 files reviewed, 6 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

---

src/Model.cc, line 422 at r2 (raw file):

Previously, scpeters (Steve Peters) wrote…

that works for now, but it will change when nested model support is merged

for example:
https://github.com/osrf/sdformat/pull/316/files#diff-a2d1f1ae2599df4eafdd728704bf7fa6

can you add a TODO about that?

I think my comment is wrong about assuming that all model frames are specified relative to their parent frame, but we should still be able to use RawPose here because we don't care about its relative_to. The adjusted model frame pose will have the same relative_to as the original. Symbolically, if the relative_to frame is R instead of Mp, we'd have X_RM = X_RPf * inv(X_MPf) where X_RM is the adjusted model frame pose, X_RPf is the original model frame RawPose and X_MPf is the resolved pose of the placement frame relative to it's model frame.

[azeey]

Reviewable status: 12 of 16 files reviewed, 5 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

---

sdf/1.8/model.sdf, line 18 at r2 (raw file):

Previously, scpeters (Steve Peters) wrote…

nit: add a . at the end of the sentence

Done. 71fc0dd

---

src/Model.cc, line 422 at r2 (raw file):

Previously, azeey (Addisu Taddese) wrote…

I think my comment is wrong about assuming that all model frames are specified relative to their parent frame, but we should still be able to use RawPose here because we don't care about its relative_to. The adjusted model frame pose will have the same relative_to as the original. Symbolically, if the relative_to frame is R instead of Mp, we'd have X_RM = X_RPf * inv(X_MPf) where X_RM is the adjusted model frame pose, X_RPf is the original model frame RawPose and X_MPf is the resolved pose of the placement frame relative to it's model frame.

I added some tests to show this, but haven't updated the comment. d8dde16

---

src/parser.cc, line 1041 at r2 (raw file):

Previously, scpeters (Steve Peters) wrote…

spacifying

specifying

Done. 71fc0dd

---

src/parser.cc, line 1275 at r2 (raw file):

Previously, scpeters (Steve Peters) wrote…

direclty

one more

Done. 71fc0dd

---

test/integration/nested_model.cc, line 886 at r2 (raw file):

Previously, scpeters (Steve Peters) wrote…

there's a windows compiler warning saying this line is unreachable

https://build.osrfoundation.org/job/sdformat-ci-pr_any-windows7-amd64/1494/msbuild/new/

Hopefully, this will fix it d8dde16

[scpeters]

Reviewed 2 of 4 files at r3.
Reviewable status: 14 of 16 files reviewed, 3 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

[scpeters]

Reviewable status: 14 of 16 files reviewed, 4 unresolved discussions (waiting on @azeey and @EricCousineau-TRI)

---

src/Model.cc, line 422 at r2 (raw file):

Previously, azeey (Addisu Taddese) wrote…

I added some tests to show this, but haven't updated the comment. d8dde16

I think you're right, and those were good tests to add. Can you update this comment accordingly?

---

test/integration/nested_model.cc, line 886 at r2 (raw file):

Previously, azeey (Addisu Taddese) wrote…

Hopefully, this will fix it d8dde16

ok, looks like it did

---

test/integration/nested_model.cc, line 1010 at r3 (raw file):

      "model_with_joint_placement_frame", "J2");

  // Test that the pose of an included model with placement_frame can use the

this comment is inaccurate, it's not an included model

---

test/sdf/placement_frame.sdf, line 71 at r3 (raw file):

    </frame>
    <include>
      <name>include_with_placement_frame_and_pose_relative_to</name>

nit: can we call this nested_include_with_placement_frame_and_pose_relative_to to distinguish from the one included in the world scope?

[scpeters]

Reviewable status: 14 of 16 files reviewed, 4 unresolved discussions (waiting on @azeey and @EricCousineau-TRI)

a discussion (no related file):
FYI: the Ubuntu CI cmake warning should be fixed by #328

---

[azeey]

Reviewable status: 13 of 16 files reviewed, 3 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

---

src/Model.cc, line 422 at r2 (raw file):

Previously, scpeters (Steve Peters) wrote…

I think you're right, and those were good tests to add. Can you update this comment accordingly?

Done. af0cf95

---

test/integration/nested_model.cc, line 1010 at r3 (raw file):

Previously, scpeters (Steve Peters) wrote…

this comment is inaccurate, it's not an included model

Done. af0cf95

---

test/sdf/placement_frame.sdf, line 71 at r3 (raw file):

Previously, scpeters (Steve Peters) wrote…

nit: can we call this nested_include_with_placement_frame_and_pose_relative_to to distinguish from the one included in the world scope?

Done. af0cf95

[EricCousineau-TRI]

with some minor comments

Reviewed 5 of 11 files at r1, 5 of 11 files at r2, 2 of 4 files at r3, 3 of 3 files at r4.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on @azeey and @scpeters)

---

src/FrameSemantics.cc, line 1311 at r4 (raw file):

    auto &edge = incidentsTo.begin()->second;
    _graph.graph.AddEdge({edge.get().Tail(), edge.get().Head()}, _pose);
    _graph.graph.RemoveEdge(edge.get().Id());

nit Adding then removing feels like an odd ordering choice.
Consider reversing those two lines?

---

src/FrameSemantics.cc, line 1321 at r4 (raw file):

  else
  {
    errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,

nit Can you add a comment mentioning what situation this "physical" ties to?

(how might a user run into this?)
It's hard for me to know this by glancing at the code.

---

src/Model.cc, line 439 at r4 (raw file):

        // just updating childModelFrame doesn't update the pose graph.
        //
        // X_RM = X_RPf * inv(X_MPf)

nit Rather that writing the math in comments, consider just using X_RPf instead of placementFramePose, X_MPf instead of placementFrameRelPose, etc.?

Then the math is obvious, IMO?

---

src/parser.cc, line 1280 at r4 (raw file):

      // TODO (addisu) Remove placementFrameIdentifier once PR addressing
      // https://github.com/osrf/sdformat/issues/284 lands
      ElementPtr placementFrameIdentifier = _sdf->AddElement("frame");

nit The name Identifier sounds like a bit of a misnomer. I expected it to be a string, "placement_frame", but find that it's a full element.

Consider another name, like placementFrameShimElement, to explicitly denote that it's just "filling in"?

[scpeters]

Reviewed 1 of 3 files at r4.
Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @azeey and @EricCousineau-TRI)

---

src/FrameSemantics.cc, line 1311 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Adding then removing feels like an odd ordering choice.
Consider reversing those two lines?

it needs to reference the old edge's head and tail before removing I think

---

src/FrameSemantics.cc, line 1321 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Can you add a comment mentioning what situation this "physical" ties to?

(how might a user run into this?)
It's hard for me to know this by glancing at the code.

I think it would mean that this graph has an error because there are multiple ways to define the pose of a given frame, when there should only be 1

[scpeters]

LGTM

Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @azeey and @EricCousineau-TRI)

[azeey]

Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

---

src/FrameSemantics.cc, line 1311 at r4 (raw file):

Previously, scpeters (Steve Peters) wrote…

it needs to reference the old edge's head and tail before removing I think

Yeah, it needs to reference the old edge. I might be able to get around it by storing the tail and head first.

---

src/FrameSemantics.cc, line 1321 at r4 (raw file):

Previously, scpeters (Steve Peters) wrote…

I think it would mean that this graph has an error because there are multiple ways to define the pose of a given frame, when there should only be 1

This would be like an element having two //pose elements with different //pose/@relative_to attributes. I don't think it can occur in a parsed in SDFormat file. Maybe if the graph was manipulated manually?

---

src/Model.cc, line 439 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Rather that writing the math in comments, consider just using X_RPf instead of placementFramePose, X_MPf instead of placementFrameRelPose, etc.?

Then the math is obvious, IMO?

It would go against our style guide, but I think in this case it would make things clearer, so I'm up for it.

[scpeters]

Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @azeey, @EricCousineau-TRI, and @scpeters)

---

src/FrameSemantics.cc, line 1321 at r4 (raw file):

Previously, azeey (Addisu Taddese) wrote…

This would be like an element having two //pose elements with different //pose/@relative_to attributes. I don't think it can occur in a parsed in SDFormat file. Maybe if the graph was manipulated manually?

yeah, this shouldn't happen for a graph built by sdf::buildPoseRelativeToGraph

[azeey]

Reviewable status: 13 of 16 files reviewed, all discussions resolved (waiting on @EricCousineau-TRI and @scpeters)

---

src/FrameSemantics.cc, line 1311 at r4 (raw file):

Previously, azeey (Addisu Taddese) wrote…

Yeah, it needs to reference the old edge. I might be able to get around it by storing the tail and head first.

Done. 9959445

---

src/FrameSemantics.cc, line 1321 at r4 (raw file):

Previously, scpeters (Steve Peters) wrote…

yeah, this shouldn't happen for a graph built by sdf::buildPoseRelativeToGraph

Done. 9959445

---

src/Model.cc, line 439 at r4 (raw file):

Previously, azeey (Addisu Taddese) wrote…

It would go against our style guide, but I think in this case it would make things clearer, so I'm up for it.

Done. 9959445

---

src/parser.cc, line 1280 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit The name Identifier sounds like a bit of a misnomer. I expected it to be a string, "placement_frame", but find that it's a full element.

Consider another name, like placementFrameShimElement, to explicitly denote that it's just "filling in"?

Yeah, that's a better name. Thanks. 9959445

[scpeters]

Reviewed 3 of 3 files at r5.
Reviewable status: complete! all files reviewed, all discussions resolved

[EricCousineau-TRI]

Per convo, @azeey meant that #284, not #283, needs to be resolved to remove scaffolding.