Skip to content
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

Force Visualization #1898

Open
wants to merge 33 commits into
base: gz-sim7
Choose a base branch
from

Conversation

srmainwaring
Copy link
Contributor

@srmainwaring srmainwaring commented Feb 15, 2023

🎉 New feature

Supersedes #1184.

Targets base branch main as there is a new method in Link.h.

Summary

This updates the previous PR #1184 and resolves some of the issues with that PR outlined in #1184 (comment).

Dependencies

Tasks

There are still a number of tasks to complete:

Tasks for this PR

  • Complete the RemoveArrow function to remove up visuals when required.

Tasks for follow up PR(s)

  • Add throttle for the EntityWrenchMap component update in Link.
  • Ensure the Qt GUI element syncs correctly on first load.
  • Fix visible flag updates.
  • Fix colour updates.
  • Fix arrow heads (they are hidden as they are not scaled correctly).
  • Add torque visual.
  • Add force and torque text.

Test it

The lift-drag example has been modified to help with testing. From the workspace folder run:

gz sim -v4 -r

and select Visualize Forces from the GUI plugin menu.

In another terminal set the rotor velocity:

gz topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_vel" -m gz.msgs.Double -p "data: 430.0"

visualize_forces_lift_drag

The plugin may be configured in the GUI config with the element:

<plugin filename="VisualizeForces" name="Visualize Forces">
    <gz-gui>
        <title>Visualize Forces</title>
    </gz-gui>
    <scale>1</scale>
    <update_rate>-1</update_rate>
</plugin>

Checklist

  • Signed all commits for DCO
  • Added tests
  • Added example and/or tutorial
  • Updated documentation (as needed)
  • Updated migration guide (as needed)
  • Consider updating Python bindings (if the library has them)
  • 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 and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

@osrf-triage osrf-triage added this to Inbox in Core development Feb 15, 2023
@srmainwaring srmainwaring marked this pull request as draft February 15, 2023 21:22
@arjo129
Copy link
Contributor

arjo129 commented Feb 16, 2023

Hmmm the rebase didnt work as expected 💀. Going to be very hard for reviewers to review. I think cherry-picking would be an alternative.

https://github.com/arjo129/gz-sim

I don't use this fork anymore.

There should be a visual to display the torque (the rotation element of arrow, scaled correctly?).

Can be a seperate PR.

@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 16, 2023

The rebase looks terrible because the target branch for the original PR is about 2y old. I rebased my fork of that on gz-sim7 first then made changes. If you were to rebase the original PR it looks a lot better. Or I could make this PR target main? It depends on how you’d like to keep the history. I’ll prob squash most of my changes into one or two commits. Could do the same for your original history which would clean things up?

Update

Switched base branch from arjo/forceviz to main (gz-sim8). Changes are much easier to see. Squashed my commits, retained history from original PR.

@srmainwaring srmainwaring changed the base branch from arjo/forceviz to gz-sim7 February 16, 2023 08:04
@srmainwaring srmainwaring changed the base branch from gz-sim7 to main February 16, 2023 14:22
@srmainwaring srmainwaring force-pushed the arjo/forceviz branch 3 times, most recently from 7852902 to 0fa43d0 Compare February 16, 2023 14:43
@codecov
Copy link

codecov bot commented Feb 16, 2023

Codecov Report

Merging #1898 (1354cd7) into gz-sim7 (753ec8d) will increase coverage by 0.03%.
The diff coverage is 80.64%.

❗ Current head 1354cd7 differs from pull request most recent head b72f045. Consider uploading reports for the commit b72f045 to get more accurate results

@@             Coverage Diff             @@
##           gz-sim7    #1898      +/-   ##
===========================================
+ Coverage    64.54%   64.58%   +0.03%     
===========================================
  Files          347      348       +1     
  Lines        27777    27865      +88     
===========================================
+ Hits         17930    17997      +67     
- Misses        9847     9868      +21     
Impacted Files Coverage Δ
include/gz/sim/Link.hh 100.00% <ø> (ø)
src/gui/plugins/spawn/Spawn.cc 9.88% <0.00%> (-0.40%) ⬇️
src/Link.cc 92.85% <83.33%> (-1.92%) ⬇️
include/gz/sim/components/EntityWrench.hh 100.00% <100.00%> (ø)
src/systems/buoyancy/Buoyancy.cc 82.47% <100.00%> (+0.22%) ⬆️
src/systems/buoyancy_engine/BuoyancyEngine.cc 87.62% <100.00%> (+0.12%) ⬆️
src/systems/hydrodynamics/Hydrodynamics.cc 84.88% <100.00%> (+0.06%) ⬆️
src/systems/lift_drag/LiftDrag.cc 63.27% <100.00%> (+0.20%) ⬆️
...s/multicopter_motor_model/MulticopterMotorModel.cc 76.20% <100.00%> (+0.29%) ⬆️
src/systems/physics/Physics.cc 67.22% <100.00%> (+0.87%) ⬆️

... and 1 file with indirect coverage changes

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

@azeey azeey moved this from Inbox to In progress in Core development Feb 16, 2023
@azeey azeey removed their request for review February 18, 2023 15:40
@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 19, 2023

Updated to fix an issue where only one force per entity was visible. Link publishes a component that maintains a std::unordered_map of wrenches keyed by the force label. The original version used a message containing a vector of wrenches with each new force being pushed into the vector if the component was available. The problem was that it was not clear which agent would clear the vector, so it continued to fill without bound. Using a map ensures that the component holds only the number of forces applied to each link. The GUI extracts all the forces and queues them for processing in the render loop.

There are now methods to remove visuals and clear them when the GUI plugin unloads. Example usage with multiple forces applied to the MBARI buoy:

visualize_forces_buoy

@srmainwaring
Copy link
Contributor Author

@arjo129 - this might be working well enough for a first version? Could target either main or gz-sim7 depending on whether the changes to Link and the addition of the component are considered ABI breaking or not.

Some questions:

  • Should the the system plugins specify whether or not to show visuals (like sensors)? I'd say yes because it will be more efficient, but requires a new field for all system plugins that apply forces.
  • How to apply a throttle - the wrench function in Link doesn't have gz::sim::UpdateInfo as an argument so any sim time based rate control would need to be in each system plugin. Or, you could put a counter and publish every n-th sample (rather than use a duration). I don't have a strong view either way - perhaps follow the approach used in sensors?
  • We might want to scale the forces from different system plugins differently. For instance buoyancy can have large forces compared to hydrodynamic drag - scaling all forces equally means that the detail may be lost. Perhaps a GUI configurable for a second round?
  • There's a few things to do in the GUI, such as make the colors and visibility tags active. Even without these the plugin is useful as is.

@arjo129 arjo129 self-requested a review February 20, 2023 07:34
@arjo129
Copy link
Contributor

arjo129 commented Feb 20, 2023

That's awesome I'll review it tomorrow. Will cherry pick the commits onto a new branch fi you don't mind.

How to apply a throttle - the wrench function in Link doesn't have gz::sim::UpdateInfo as an argument so any sim time based rate control would need to be in each system plugin. Or, you could put a counter and publish every n-th sample (rather than use a duration). I don't have a strong view either way - perhaps follow the approach used in sensors?

Seems reasonable to follow sensors.

We might want to scale the forces from different system plugins differently. For instance buoyancy can have large forces compared to hydrodynamic drag - scaling all forces equally means that the detail may be lost. Perhaps a GUI configurable for a second round?

Yep. We can revisit this in some future PR.

There's a few things to do in the GUI, such as make the colors and visibility tags active. Even without these the plugin is useful as is.

Again we can slate this as "future work"

@srmainwaring
Copy link
Contributor Author

Will cherry pick the commits onto a new branch fi you don't mind

Sure - no prob. I also have a branch based against gz-sim7 as well here (https://github.com/srmainwaring/gz-sim/tree/srmainwaring/7/forceviz) if that helps. Both build on your original PR (but rebased).

Seems reasonable to follow sensors.

I'll look at the approach used - it will probably mean additions to the SDF for force providing system plugins. Adding a function to toggle publishing the wrench component in Link and some boiler plate to activate / deactivate that based on the duration since last publish in the system plugins should work.

@arjo129
Copy link
Contributor

arjo129 commented Feb 20, 2023 via email

@srmainwaring
Copy link
Contributor Author

I think it'd be great if you opened the PR using the rebased branch.

Actually this PR already uses the rebased branch (targeting main). I can also target gz-sim7 if you prefer.

@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 20, 2023

Noticed a bug: if there are multiple instances of a plugin generating forces for the same same link, then only one will contribution will be picked up. Example: lift-drag plugin for the iris quadcopter where two instances of the LiftDrag system generate forces for the same link.

Update

Also - something very odd (again). The _ecm.Each< in the visual does not seem to be picking up updates correctly (again) even though I can trace the component::EntityWrenches being serialised on the server and deserialised on the gui. It's very confusing - any suggestions what might be going on? This seems to have happened when I changed the component to contain an unordered map rather than a single message.

@arjo129 arjo129 changed the base branch from main to gz-sim7 February 20, 2023 17:17
@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 20, 2023

@arjo129 changed the base branch from main to gz-sim7 [31 minutes ago]

I'll update the branch on my side to be based in gz-sim7 also - that will need a force push for this PR. Does this mean there are no ABI issues with these changes?

@arjo129
Copy link
Contributor

arjo129 commented Feb 20, 2023

My understanding is that there are no breaking abi changes. IIRC Adding a member function should not lead to ABI incompatibilities. Same applies for components. We have added components in minor releases in the past (not ideal). Ideally we would target main given that we may want to go back and break the ABI, but given the way git and github work it would have been impossible to determine the changes for reviewers on the PR without cherry picking commits.

Noticed a bug: if there are multiple instances of a plugin generating forces for the same same link, then only one will contribution will be picked up. Example: lift-drag plugin for the iris quadcopter where two instances of the LiftDrag system generate forces for the same link.

I can't think of a way to solve this without massive changes in other places. Perhaps we should provide an option within sdf of liftdrag, buoyancy and hydrodynamics to specify<force_debug_namespace>?

@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 20, 2023

given the way git and github work it would have been impossible to determine the changes for reviewers on the PR

No, it would have been fine. I'd just rebase the entire history starting with your changes on main and force push.

git rebase --onto main gz-sim7 arjo/forceviz

In any case either main or gz-sim7 works for me. The latter is a little more convenient as harmonic is still WIP as far as deps are concerned so its a bit more work to get a source build on macOS.

The issues with updates from the ECM are proving very confusing. The current version is broken, I fixed one thing (multiple forces for one entity), but seem to have broken the updates that were working when I was using a simple message as the component payload. It will take a bit more digging to figure out why the GUI is getting the updates but not finding them in the templated Each loop. I'm looking at VisualizationCapabilities which seems to be the GUI plugin closest to what we need for force visualization - but can't see anything obvious yet.

I'll push the debug code for the component serializer / deserializer (commented out) so you can see what I mean. I find it helps to set the RTF very low (RTF = 0.01 or less) when debugging to avoid getting swamped with messages.

@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 20, 2023

Perhaps we should provide an option within sdf of liftdrag, buoyancy and hydrodynamics to specify<force_debug_namespace>?

Yeah that might work - it's not super urgent as you can introduce another link (this is what the lift_drag example does which is why I didn't spot it initially).

The biggest issue is the broken update - which I thought I had solved, but managed to break again. That's a blocker for this at the moment.

Update

3aa106602 adds debugging info using the previous approach of a component containing a single EntityWrench for comparison with using a component containing an unordered_map. Both components are published in Link and the GUI can toggle the #if - def compilation flags to process either one. The single message updates correctly, the map of messages updates once only.

- Use new message type msgs::EntityWrenchMap.
- Disable  components::EntityWrench and components::EntityWrenches.
- Enable components::EntityWrenchMap.

Signed-off-by: Rhys Mainwaring <[email protected]>
- Remove unused components.
- Remove debug / prototyping code from Link and VisualizeForces.

Signed-off-by: Rhys Mainwaring <[email protected]>
@srmainwaring srmainwaring marked this pull request as ready for review February 21, 2023 13:17
@srmainwaring
Copy link
Contributor Author

I can't think of a way to solve this without massive changes in other places. Perhaps we should provide an option within sdf of liftdrag, buoyancy and hydrodynamics to specify<force_debug_namespace>?

Here's an approach along those lines that might work: srmainwaring@d192a43.

I've also added throttling to the LiftDrag plugin on the publication side to reduce bandwidth usage.

Figure: force visualization with namespaces applied to SkyCat TVBS where each prop has two force contributions and all other lift elements are associated with the base link.

visualize_forces_skycat-tvbs

Probably for a follow-up PR, but I think it might be useful to move the force publication out of the Link:

  • Create standalone class to manage publishing entity forces for visualization.
  • May want to see the force - torque at the point of application rather than at the origin of the link frame.
  • Optionally publish messages to a topic for analysis in external tools.
  • A separate class could process the additional SDF elements (under a element) to reduce copying boilerplate to each system plugin.

As a tool this is quite useful already. I have been trying to get a stable version of a 6DoF omnicopter running in Gazebo: https://github.com/srmainwaring/SITL_Models/blob/ignition/omnicopter/Gazebo/docs/Omnicopter.md
and I'm finding the tune very difficult. I have suspected that using the LiftDrag plugin for props causes a lot of noise - and the visualization appears to support this. Having the data published for plotting in an external tool would help complete the analysis.

Copy link
Contributor

@arjo129 arjo129 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the amazing work @srmainwaring. I just tried it. Overall it looks great. There are still some minor annoyances that we have to deal with.

  • UI doesn't load the first time. Not sure why but you have to collapse the panel and then re-open the panel. I'm not sure if the issue is with the code I wrote early on or some weird Qt/GzGui interaction. We can ticket this and fix it as we iterate through the feature.
  • Gravity does not appear. I think this is trivial to solve, again we can revisit this in the future. I remember a throwaway branch (here: https://github.com/gazebosim/gz-sim/tree/arjo/feat/force_debug) where I simply iterate through inertial components. Again this can be an add-on feature.

Both of these can be added as follow up PRs.

src/Link.cc Show resolved Hide resolved
examples/worlds/lift_drag.sdf Show resolved Hide resolved
Core development automation moved this from In progress to In review Feb 22, 2023
@srmainwaring
Copy link
Contributor Author

srmainwaring commented Feb 22, 2023

@arjo129 this branch (https://github.com/srmainwaring/gz-sim/tree/arjo/forceviz-throttled) shows how we could move the force publishing out of the Link class in a follow up PR. It adds some features that are hard to deal with in the current version.

  • Add throttle when publishing forces.
  • Publish to a topic as well, for external analysis.
  • Add time stamps to the messages.
  • Remove messages from the map that are out of date (so all the forces published are for the same time step).

I've made the changes in the LiftDrag plugin, adding a ForcePublisher class which would be moved to it's own file if we wanted to proceed.

@arjo129
Copy link
Contributor

arjo129 commented Feb 23, 2023

Interesting. One alternative would be to have the forcedrag plugin handle the publication as opposed to a force publisher. That would remove the need for additional API.
Additionally we could zero out the map like we do in the physics system.
I guess there are pros and cons to each method.

@arjo129 arjo129 added needs upstream release Blocked by a release of an upstream library 🌱 garden Ignition Garden labels Feb 23, 2023
@iche033
Copy link
Contributor

iche033 commented Mar 10, 2023

I think this is just blocked by a msgs release? If so, I can do a gz-msgs9 release

@iche033
Copy link
Contributor

iche033 commented Mar 10, 2023

@osrf-jenkins run tests please

Copy link
Contributor

@iche033 iche033 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

gz-msgs9 was released and CI should be working now. I merged with gz-sim7 to fix ABI build.

Force visualization in lift_drag.sdf works for me. I made some minor style comments and a suggestion about function naming.

/// \brief Last system update simulation time
public: std::chrono::steady_clock::duration lastUpdateTime{0};


Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove extra line


public: void RemoveVisual(const std::string &_ns);

public: void ClearVisuals();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add doxygen for these functions

@@ -306,6 +306,11 @@ namespace gz
const math::Vector3d &_force,
const math::Vector3d &_torque) const;

/// \brief Sets the visualization label used by the force visualization.
/// \param[in] _label The label used for force visualizations.
public: void SetVisualizationLabel(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's not clear what this function does from the name and doc. At first glance I thought this will visualize a text label next to the link in the gui. After reading through the code, I think it assigns a unique color for any force visualizations that have the same label string?

Maybe rename this to something more descriptive? e.g. SetForceVisualizationGroup or SetForceVisualizationLabel and add a brief description about what the arg does on the gui side.

*/

#ifndef GZ_SIM_GUI_VISUALIZECONTACTS_HH_
#define GZ_SIM_GUI_VISUALIZECONTACTS_HH_
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

VISUALIZECONTACTS -> VISUALIZEFORCES

@azeey
Copy link
Contributor

azeey commented Apr 10, 2023

@srmainwaring Have you had a chance to take a look at the feedback?

@srmainwaring
Copy link
Contributor Author

Have you had a chance to take a look at the feedback?

On my todo list. Apologies for the delay, I'll try to incorporate the changes in the next few days.

@arjo129
Copy link
Contributor

arjo129 commented Jul 28, 2023

#2051 Has a nice visual. I wonder if we should extract the visualization component from it out else where.

@azeey azeey added the beta Targeting beta release of upcoming collection label Jul 31, 2023
@azeey azeey removed the beta Targeting beta release of upcoming collection label Aug 21, 2023
Copy link
Contributor

@ahcorde ahcorde left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you fix the conflicts ? are you planning to finish this PR @srmainwaring ?

@srmainwaring
Copy link
Contributor Author

srmainwaring commented Jan 17, 2024

can you fix the conflicts ? are you planning to finish this PR @srmainwaring ?

Hi @ahcorde - forgot about this PR - let me review what's needed to get it ready. It may not be in this branch, but I had a more general way of enabling / disabling the force visualisation that wasn't so intrusive when not required. The visuals from the apply force plugin look good, so I'll look at what's involved in adopting those while I'm at it.

@azeey
Copy link
Contributor

azeey commented Jun 10, 2024

@srmainwaring do we need to close this PR and create a new one for the other approach you mentioned?

@azeey azeey added beta Targeting beta release of upcoming collection and removed beta Targeting beta release of upcoming collection labels Jul 29, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
🌱 garden Ignition Garden needs upstream release Blocked by a release of an upstream library
Projects
Status: In progress
Development

Successfully merging this pull request may close these issues.

None yet

5 participants