Add linear velocity components to links that have linear velocity cmds applied

[adlarkin]

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

🦟 Bug fix

Fixes #966

Summary

When a link has a LinearVelocityCmd applied to it in a given simulation step, there's no way to read what the link's linear velocity is in future simulation steps since the LinearVelocityCmd is "zeroed out" after it is applied. This PR adds a LinearVelocity and WorldLinearVelocity component to a link when a LinearVelocityCmd is applied to it, so that users have a way to read a link's linear velocity in future simulation steps. I've added a test case to check this in the physics system integration test, and have verified that running the test to read linear velocity data from a link with a LinearVelocityCmd applied to it fails without the other changes made in this PR.

We should probably make similar updates for LinearVelocityCmds applied to models, but this will be easier to do starting in ign-gazebo5 since we can make use of #736 and #685 (if a LinearVelocityCmd is applied to a model and we want to create LinearVelocity and WorldLinearVelocity components for the model, we could populate the data in these components with the new velocity information from the model's canonical link).

On a somewhat related note to this, we could also make similar changes for AngularVelocityCmd, so that users who apply AngularVelocityCmds can read the corresponding entity's angular velocity data in future simulation steps. We could either open a new PR for this, or add it to this one if this change seems like something worthwhile.

Checklist

• Signed all commits for DCO
• Added tests
• 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

[azeey]

If the user wants the LinearVelocity of a link, shouldn't they create the component themselves? I say this because there are various components that are not created by default and the pattern we've been following so far is to expect users to create the components they need.

[adlarkin]

If the user wants the LinearVelocity of a link, shouldn't they create the component themselves?

I'm not opposed to this, but if we expect the user to create this component themselves, then how can they accurately populate the data for this component? They could set the data of the LinearVelocity component to be the data of the LinearVelocityCmd component, but the actual velocity vs. the commanded velocity will differ slightly due to physics forces, right? If we want the LinearVelocity component to have the "true" linear velocity of an entity, then I believe that the physics system would need to create this since the users don't have access to the output frame data from physics (or at least, I don't think they have access).

---

EDIT: Actually, what I said above does not matter. If the user creates a LinearVelocity component for the link that they applied a LinearVelocityCmd to, then the physics system will take care of updating this as needed (https://github.com/ignitionrobotics/ign-gazebo/blob/ignition-gazebo3_3.9.0-pre2/src/systems/physics/Physics.cc#L1922-L1934). So, I believe that the proper solution is to have users create a LinearVelocity component for links that they want to keep track of, and then read from that component to get the up-to-date linear velocity. I guess that means we can close this, right @azeey?

[azeey]

Maybe we don't have this documented very well, but in general, when users want a value like LinearVelocity, they create the component on the entity in the PreUpdate. The physics system will then know to populate the component from the physics engine in
https://github.com/ignitionrobotics/ign-gazebo/blob/0c401c17c1d4b748233c61921c47da5725ce9d5c/src/systems/physics/Physics.cc#L2432-L2563

[adlarkin]

Maybe we don't have this documented very well, but in general, when users want a value like LinearVelocity, they create the component on the entity in the PreUpdate. The physics system will then know to populate the component from the physics engine

Yeah, I just realized this - see my edit on #992 (comment). So, should this PR be closed? If so, I can post on #992 that the solution is to create a LinearVelocity component for links of interest.

[chapulina]

Maybe we don't have this documented very well, but in general, when users want a value like LinearVelocity, they create the component on the entity in the PreUpdate

For beginners, my recommendation would be to encourage them to use Link::EnableVelocityChecks, which creates the necessary components. But I agree that it's a good idea to document that thee components should be created, for power users.

[codecov]

Codecov Report

Merging #992 (df973b1) into ign-gazebo3 (054ff12) will increase coverage by 0.00%.
The diff coverage is 100.00%.

@@             Coverage Diff              @@
##           ign-gazebo3     #992   +/-   ##
============================================
  Coverage        77.84%   77.84%           
============================================
  Files              221      221           
  Lines            12687    12689    +2     
============================================
+ Hits              9876     9878    +2     
  Misses            2811     2811           

Impacted Files	Coverage Δ
src/systems/physics/Physics.cc	70.40% <100.00%> (+0.06%)	⬆️

---

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 054ff12...df973b1. Read the comment docs.

[adlarkin]

I guess my only other remark is that it looks like we currently don't have code in the physics system that would take care of updating a model's velocity if a LinearVelocity component was attached to a model. It looks like we have a way to handle links, sensors and collisions. However, I guess that if a user wanted to know a model's velocity/acceleration/etc., they could just look at the model's canonical link, right?

[luca-della-vedova]

I agree with not adding LinearVelocity components automatically, building up on what @adlarkin mentioned, I wonder if you think it's a sensible default to assign to a Model the velocity of its canonical link. For reference this was the sample system I created for the issue.
It works once I change the entity from the model's to the canonical link's. I think there is a bit of disconnect since the LinearVelocityCmd can be applied to the model, and when it is the model itself moves, but the model's LinearVelocity component isn't updated and always reads back as zero.
I guess it can be made more coherent in two ways, either make it impossible to apply LinearVelocityCmd to models and force users to only work at the canonical link level, or make sure that when LinearVelocityCmd components are applied to models the model's LinearVelocity component is also updated.
I personally prefer the second since I think defaulting a model's velocity to that of its canonical link is a reasonable assumption, but to be fully honest I never heard of canonical links until a few days ago so I don't know if it makes sense 😅

[adlarkin]

I think there is a bit of disconnect since the LinearVelocityCmd can be applied to the model, and when it is the model itself moves, but the model's LinearVelocity component isn't updated and always reads back as zero.

The reason why this is the current behavior is because in the physics system, we get updated frameData for links that have changed. But yes, I agree that this is confusing 😬

I guess it can be made more coherent in two ways, either make it impossible to apply LinearVelocityCmd to models and force users to only work at the canonical link level, or make sure that when LinearVelocityCmd components are applied to models the model's LinearVelocity component is also updated.

I am not opposed to applying a LinearVelocityCmd to a model and then updating the model's LinearVelocity component if it exists - we'd just need to figure out how to implement this functionality (right now, nothing immediate comes to mind regarding implementation, but perhapsit wouldn't be too hard to implement). For now, I think that using the canonical link should be fine, since I understand the canonical link to serve as a "reference" for the model's velocity, pose, acceleration, etc.

@azeey what do you think about adding functionality for updating LinearVelocityCmd components that are attached to models? Do you envision any challenges with implementing this?

[azeey]

I'm on board with setting LinearVelocityCmd on models and updating the LinearVelocity to reflect the actual velocity of the model frame. However, note that the model frame is not the same as canonical link frame since there can be a transform between the two. For example:

<model name="M">
  <link name="L">
        <pose>1 0 0   0 0 0</pose>
   </link>
</model>

The model frame of M is translated from the canonical link by -1 0 0.

The reason we he haven't implemented updates to the LinearVelocity component of models is because the dartsim plugin of ign-physics does not make the frame data readily available for models because it doesn't implement ModelFrameSemantics. But if we want to do it, we can compute the frame data for the model frame from the frame data of the canonical link in ign-gazebo.

[iche033]

make sure that when LinearVelocityCmd components are applied to models the model's LinearVelocity component is also updated

gazebo-classic does something similar - when vel cmd is set on the model, it applies to all the links and when user requests to get vel, it returns the canonical link's vel:
https://github.com/osrf/gazebo/blob/gazebo11/gazebo/physics/Model.cc#L733
https://github.com/osrf/gazebo/blob/gazebo11/gazebo/physics/Model.cc#L761

[adlarkin]

But if we want to do it, we can compute the frame data for the model frame from the frame data of the canonical link in ign-gazebo.

Sounds reasonable to me 👍 @luca-della-vedova, for RMF, are you guys okay with using canonical links for now, or would you prefer to use command velocities on models? If adding functionality for updating a model's LinearVelocity component isn't urgent, I can create an issue for it and then one of us can address it whenever we have time for it.

[luca-della-vedova]

But if we want to do it, we can compute the frame data for the model frame from the frame data of the canonical link in ign-gazebo.

Sounds reasonable to me +1 @luca-della-vedova, for RMF, are you guys okay with using canonical links for now, or would you prefer to use command velocities on models? If adding functionality for updating a model's LinearVelocity component isn't urgent, I can create an issue for it and then one of us can address it whenever we have time for it.

Sounds good, we can use the canonical links for now!

[adlarkin]

we can use the canonical links for now

Okay, thanks! I have opened an issue so that we can track the status of adding LinearVelocity component support for models (#998). I marked it as "help wanted" for now, but hopefully someone can get around to it once they have time.

---

I am going to close this PR since it's agreed that it's up to users to generate the LinearVelocity and WorldLinearVelocity components if they want this information.