write modified link data to output in physics step #223

adlarkin · 2021-03-10T03:32:18Z

Signed-off-by: Ashton Larkin ashton@openrobotics.org

🎉 New feature

Possible solution for #219

Summary

As mentioned in #219, downstream libraries that use ign-physics (like ign-gazebo) don't have a way of knowing which links moved in the most recent physics step. This PR takes advantage of a forward step's output by writing changed link poses to the output. The output can then be used by downstream libraries to only operate on modified links (take a look at gazebosim/gz-sim#678 for an example).

Test it

Take a look at the tests added in this PR (files changed tab -> tpe/plugin/src/SimulationFeatures_TEST.cc or dartsim/src/SimulationFeatures_TEST.cc) to see how the modified pose data can be accessed from ignition::physics::ForwardStep::Output.

To get an understanding of how this change can be used/tested in downstream libraries, take a look at gazebosim/gz-sim#678.

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
codecheck passed (See contributing)
All tests passed (See test coverage)
While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

codecov · 2021-03-10T03:46:20Z

Codecov Report

Merging #223 (3cd0d7b) into main (109de85) will increase coverage by 0.14%.
The diff coverage is 97.50%.

@@            Coverage Diff             @@
##             main     #223      +/-   ##
==========================================
+ Coverage   82.85%   83.00%   +0.14%     
==========================================
  Files         107      107              
  Lines        4119     4159      +40     
==========================================
+ Hits         3413     3452      +39     
- Misses        706      707       +1

Impacted Files	Coverage Δ
tpe/plugin/src/SimulationFeatures.cc	`78.33% <95.00%> (+8.33%)`	⬆️
dartsim/src/SimulationFeatures.cc	`91.22% <100.00%> (+4.74%)`	⬆️

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 109de85...3cd0d7b. Read the comment docs.

adlarkin

@azeey the implementation is working, but I have a few questions. Would you mind taking a look at the code and addressing my comments below?

dartsim/src/SimulationFeatures.cc

tpe/plugin/src/SimulationFeatures.cc

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

adlarkin · 2021-03-16T17:23:03Z

This is now ready for review. I have updated this PR to target Edifice (main branch) based on the discussion that was had in gazebosim/gz-sim#678.

We'll need to make a minor release for ign-math6 so that gazebosim/gz-math#196 can be used.

iche033 · 2021-03-16T23:33:17Z

dartsim/src/SimulationFeatures.cc

+        _poses.entries.push_back(wp);
+      }
+
+      prevLinkPoses[id] = wp.pose;


do you need to keep this map up-to-date when links are removed?

Yeah, thanks for catching this. I believe I have handled this in 84b9f4e. Let me know what you think.

dartsim/src/SimulationFeatures.cc

tpe/plugin/src/SimulationFeatures.cc

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

azeey

Looks pretty good! I left some minor comments.

dartsim/src/SimulationFeatures.cc

azeey · 2021-03-19T14:55:59Z

dartsim/src/SimulationFeatures.cc

+        _poses.entries.push_back(wp);
+      }
+
+      newPoses[id] = wp.pose;


Should this be inside the if block since it's supposed to be collecting new poses?

The reason why I left this outside of the if block is because of floating point precision. My thought was that I should save the most recent pose in newPoses no matter what each time, since there's a small tolerance (1e-6) in the pose equality check. Since newPoses is used as prevLinkPoses the next time around, this ensures that we are comparing the newest link pose to the actual previous pose, instead of to a pose that is very close to the previous pose. Does that make sense? If you think that leaving this outside of the if block is okay, I can add a comment there that mentions what I just described to you. Otherwise, I can move this to be inside of the if block.

I would think you'd want to compare it to the last pose sent to the user though because you want to err on the side of sending too much to the user (eg. ign-gazebo). Consider, for example, if each successive time the pose changes by +1e-7, since you always compare against the previous actual pose, it will always look like there no new pose to send to the user. Over time, this could cause a significant amount of drift.

You're definitely correct. I believe that this should now be fixed in 494f6c5

dartsim/src/SimulationFeatures.cc

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

adlarkin · 2021-03-22T16:22:14Z

All review feedback has been addressed, so I am opening this for another round of review.

azeey · 2021-03-22T23:21:27Z

@osrf-jenkins run tests please

github-actions bot added the 🔮 dome Ignition Dome label Mar 10, 2021

adlarkin mentioned this pull request Mar 10, 2021

iterate through changed links only in UpdateSim gazebosim/gz-sim#678

Merged

8 tasks

chapulina added enhancement New feature or request performance Runtime performance labels Mar 10, 2021

adlarkin mentioned this pull request Mar 10, 2021

add Equal tolerance method to Quaternion gazebosim/gz-math#196

Merged

8 tasks

adlarkin added the needs upstream release Blocked by a release of an upstream library label Mar 10, 2021

adlarkin commented Mar 11, 2021

View reviewed changes

adlarkin requested a review from azeey March 11, 2021 17:54

adlarkin self-assigned this Mar 11, 2021

adlarkin force-pushed the adlarkin/write_step_output_data branch from a9b33c9 to f80f687 Compare March 12, 2021 21:18

adlarkin changed the base branch from ign-physics3 to main March 15, 2021 17:25

adlarkin force-pushed the adlarkin/write_step_output_data branch from f80f687 to e4dea17 Compare March 15, 2021 17:28

adlarkin changed the base branch from main to ign-physics3 March 15, 2021 19:04

adlarkin changed the base branch from ign-physics3 to main March 15, 2021 19:08

adlarkin added 3 commits March 15, 2021 15:19

write modified link data to output in physics step

7ec4f0c

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

added output writing implementation to DART

39a856d

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

added tests

f2febb1

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

adlarkin force-pushed the adlarkin/write_step_output_data branch from e4dea17 to f2febb1 Compare March 15, 2021 19:27

fix codecheck

b181b92

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

adlarkin requested review from iche033 and chapulina March 16, 2021 17:23

adlarkin marked this pull request as ready for review March 16, 2021 17:23

adlarkin requested a review from mxgrey as a code owner March 16, 2021 17:23

iche033 reviewed Mar 16, 2021

View reviewed changes

adlarkin added 2 commits March 17, 2021 11:20

use mutable data member instead of static variable

baeb7e6

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

sanity check for null worlds

2df9ea0

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

adlarkin added 2 commits March 17, 2021 12:20

handle removed links

84b9f4e

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

remove outdated TODO note

f2d08fe

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

chapulina added beta Targeting beta release of upcoming collection 🏢 edifice Ignition Edifice and removed 🔮 dome Ignition Dome labels Mar 18, 2021

azeey reviewed Mar 19, 2021

View reviewed changes

adlarkin and others added 3 commits March 19, 2021 14:58

review feedback

ad12778

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

update poses to account for drift

494f6c5

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

Merge branch 'main' into adlarkin/write_step_output_data

31fe122

adlarkin requested review from iche033 and azeey March 22, 2021 16:22

chapulina mentioned this pull request Mar 22, 2021

🎈 6.8.0~pre3 gazebosim/gz-math#199

Merged

7 tasks

Merge branch 'main' into adlarkin/write_step_output_data

3cd0d7b

adlarkin removed the needs upstream release Blocked by a release of an upstream library label Mar 22, 2021

azeey approved these changes Mar 23, 2021

View reviewed changes

azeey merged commit 0e2ea89 into main Mar 23, 2021

azeey deleted the adlarkin/write_step_output_data branch March 23, 2021 05:21

adlarkin mentioned this pull request Mar 23, 2021

Add "ChangedWorldPose" to ForwardStep::Output's expected data #238

Merged

8 tasks

luca-della-vedova mentioned this pull request Aug 30, 2021

Fix TPE Link velocity not being updated and Model velocity not having any effect. #289

Merged

7 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

write modified link data to output in physics step #223

write modified link data to output in physics step #223

adlarkin commented Mar 10, 2021 •

edited

Loading

codecov bot commented Mar 10, 2021 •

edited

Loading

adlarkin left a comment

adlarkin commented Mar 16, 2021 •

edited

Loading

iche033 Mar 16, 2021

adlarkin Mar 17, 2021

azeey left a comment

azeey Mar 19, 2021

adlarkin Mar 19, 2021

azeey Mar 19, 2021

adlarkin Mar 22, 2021

adlarkin commented Mar 22, 2021

azeey commented Mar 22, 2021

write modified link data to output in physics step #223

write modified link data to output in physics step #223

Conversation

adlarkin commented Mar 10, 2021 • edited Loading

🎉 New feature

Summary

Test it

Checklist

codecov bot commented Mar 10, 2021 • edited Loading

Codecov Report

adlarkin left a comment

Choose a reason for hiding this comment

adlarkin commented Mar 16, 2021 • edited Loading

iche033 Mar 16, 2021

Choose a reason for hiding this comment

adlarkin Mar 17, 2021

Choose a reason for hiding this comment

azeey left a comment

Choose a reason for hiding this comment

azeey Mar 19, 2021

Choose a reason for hiding this comment

adlarkin Mar 19, 2021

Choose a reason for hiding this comment

azeey Mar 19, 2021

Choose a reason for hiding this comment

adlarkin Mar 22, 2021

Choose a reason for hiding this comment

adlarkin commented Mar 22, 2021

azeey commented Mar 22, 2021

adlarkin commented Mar 10, 2021 •

edited

Loading

codecov bot commented Mar 10, 2021 •

edited

Loading

adlarkin commented Mar 16, 2021 •

edited

Loading