VelocityCmd components applied to models don't have any effect

[luca-della-vedova]

Environment

Ubuntu 20.04, Ignition Edifice source build (latest ign-gazebo5 branch).

Description

We relied on using LinearVelocityCmd and AngularVelocityCmd to apply velocities at the model level in our robot simulation plugins. However, I noticed that recently the commands don't have any effect.
An example SDF that we use this plugin with is here.
I ran git bisect and found out the exact commit that introduced this behavior, #678. I didn't look into the commit diff too much but my guess is that, since we are applying the velocity to a model entity and not a link entity it is skipped by that optimization PR.
I wonder if there was a recommended way to adapt our plugin or if this is something that should be fixed in the core code. I tried to fetch the model's canonical link through the ModelCanonicalLink component and applying the velocities to it but didn't have much success.

Steps to reproduce

It is arguably a bit complicated, it would require setting up the whole rmf_demos workspace, but let me know if you want detailed instructions.

[adlarkin]

Thanks for pointing out the issue. I have written a test that does fail to move a model when applying a LinearVelocityCmd component: 8afb937 (this branches off of ign-gazebo5)

However, I am unable to see a behavior change before/after #678. I have tried running this test before and after #678, but it always fails no matter which commit/release of ign-gazebo5 I use (the model never seems to move). Would you mind taking a look at the test I wrote and try to run it locally to see if you can get it to pass/fail based on #678? Also, if you see anything wrong with the test I wrote, feel free to comment on the commit that I linked.

[iche033]

oh and is the demo using dart or tpe?

[luca-della-vedova]

Thanks for pointing out the issue. I have written a test that does fail to move a model when applying a LinearVelocityCmd component: 8afb937 (this branches off of ign-gazebo5)

However, I am unable to see a behavior change before/after #678. I have tried running this test before and after #678, but it always fails no matter which commit/release of ign-gazebo5 I use (the model never seems to move). Would you mind taking a look at the test I wrote and try to run it locally to see if you can get it to pass/fail based on #678? Also, if you see anything wrong with the test I wrote, feel free to comment on the commit that I linked.

It seems the test world lin_vel_cmd.sdf was not committed, can you add it to the repo so I can try on my end?

oh and is the demo using dart or tpe?

It happened on both dart and TPE

[adlarkin]

It seems the test world lin_vel_cmd.sdf was not committed, can you add it to the repo so I can try on my end?

Sorry about that! I've added the world in f2cba0b

[adlarkin]

@luca-della-vedova I am able to run the following example that applies a command velocity to a model, which moves the model: https://github.com/ignitionrobotics/ign-gazebo/compare/adlarkin/linVelCmdTest. This example is based on the latest commit of ign-gazebo5, which means #678 is used.

@iche033 pointed out that default plugins aren't loaded if a user specifies their own plugins in a SDF file (c2834e0#r54822602). In the SDF file you provided in the issue description, it looks like you define a custom plugin, which means that you'll probably need to add in other plugins like physics in order to get desired behavior. Can you try modifying your SDF file based on the recommendations in c2834e0#r54822602 and let me know if the issue still persists for you?

---

On a side note, I'm not sure why the visual example I mentioned above is working, but the test I wrote is failing. Here's everything I've done for the test. If someone sees something wrong I'm doing in the test, please let me know! https://github.com/ignitionrobotics/ign-gazebo/compare/adlarkin/linVelCmd

[luca-della-vedova]

For reference this is the world, we actually redefine all the systems / GUI elements to be safe. I'll try your examples now and see how they behave

[luca-della-vedova]

OK it has been quite the debugging journey!
I apologize for my previous information because it was partially incorrect. It turns out writing to VelocityCmd components works, as you showed, reading is what doesn't, and is the behavior that changed.
Specifically, in our application we are simulating vehicles with a limited acceleration hence we apply speed in the format of:
v_new = v_old + dt * a, we rely, or used to, on reading the VelocityCmd components to get the velocity command that was sent in the previous iteration and this seemed to work until the PR I mentioned above (although to be honest I'm really not sure what changes in the PR might have affected this).

I think it is not quite correct to read VelocityCmd components since I would expect them to be reset at every iteration step, however I tried to do something more proper which is read LinearVelocity components but they also seem to be always read as 0 regardless of the commit, so at least reading the Cmd components had something working before.

I pushed an example that is closer to what we actually do in luca/linVelTest, the relevant code is here and I changed the world to download a model from fuel and the model has the plugin tag inside.
This code results in the having a velocity ramp before #678 while it always has a velocity of 0.01 after the PR. The previous commit (using LinearVelocity instead of LinearVelocityCmd to read the current velocity) sends 0.01 regardles.

Apologies for the wrong information but I hope this example can help, maybe we are doing something wrong and actually relied on some buggy behavior before?

[adlarkin]

we rely, or used to, on reading the VelocityCmd components to get the velocity command that was sent in the previous iteration

I think it is not quite correct to read VelocityCmd components since I would expect them to be reset at every iteration step

You're assumption is correct: looking at the physics system (ign-gazebo5.1.0), linear velocity commands are applied to models (and links) when it's time to update physics with the next step. After the update is completed, the linear velocity commands are "zeroed out". So, linear velocity commands should not be read from after a physics update.

I'm really not sure what changes in the PR might have affected this

At first glance, I'm not sure either 😂 #678 did not touch any of the code that applies/resets linear velocities via command components. So, I'll have to take a closer look at how #678 changed behavior.

Apologies for the wrong information but I hope this example can help, maybe we are doing something wrong and actually relied on some buggy behavior before?

No apology needed! It sounds like we've had buggy behavior that has gone unnoticed until now 🙃 Based on our discussion, I think there are a few things that need to be addressed:

1. Understand how iterate through changed links only in UpdateSim #678 changed (or did not change) behavior with LinearVelocityCmd
2. Make sure that the LinearVelocity component gets updated properly (you mentioned that LinearVelocity never seems to update even after applying a LinearVelocityCmd - this appears to be a bug)

I will go ahead and work on addressing the two things I mentioned above (if there's anything else you'd like me to investigate, let me know). Once everything has been addressed, the correct behavior on your end should be to set LinearVelocityCmd components on models/links that you want to move before the next physics update, and then use the LinearVelocity component after the physics update to read the most recent velocity of a model/link.

[adlarkin]

@luca-della-vedova I have proposed a fix for links in #992 that would allow for users to apply a LinearVelocityCmd to a link, and then read the link's linear velocity at any future point through the link's LinearVelocity or WorldLinearVelocity component. As we have already discussed in this thread, before #992, if a LinearVelocityCmd is applied to a link, the LinearVelocity and WorldLinearVelocity components are not created, so there's no way to retrieve the link's linear velocity in future simulation steps if needed.

I made this change in Citadel because I found that this behavior is broken for all released versions. So, what we can do is merge #992 forward to other released versions if it seems like a good solution. I have not made the fix for when LinearVelocityCmds are applied to models yet because that's a little more complicated to do in Citadel/Dome (see the PR description of #992), but I believe the same fix can be made for models fairly easily starting in Edifice.

So, for now, assuming that #992 gets merged, I believe what you can do to fix the buggy behavior in RMF is apply LinearVelocityCmds to links, and then read the LinearVelocity or WorldLinearVelocity component from the corresponding link (as I mentioned before, you shouldn't be trying to read the LinearVelocityCmd after applying it since it gets "zeroed out"). Does this make sense, and does this also appear to be a valid solution to the issue you're currently facing?

---

Also, regarding the test I wrote earlier in this thread that I couldn't get to work - I found that it was because I was applying a LinearVelocityCmd to the first model, which was the ground plane. The ground plane is static, so that's why it wouldn't move 🙃 but, I have fixed this in the test that I wrote for #992.

[luca-della-vedova]

Following up the discussion in #992, I changed the example to read the velocity components on the CanonicalLink and I am still a bit puzzled at the result.
Specifically, if you run it "as is" and turn on debug output you will notice that, while the system plugin sends a ramp with a slope of 0.01 per iteration, the model itself accelerates at a much slower pace (i.e. about 0.0002):

[Dbg] [LinVelCmdTest.cc:50] Setting velocity 0.0286956 to entity 8
[Dbg] [LinVelCmdTest.cc:50] Setting velocity 0.0288786 to entity 8
[Dbg] [LinVelCmdTest.cc:50] Setting velocity 0.0290617 to entity 8
[Dbg] [LinVelCmdTest.cc:50] Setting velocity 0.0292449 to entity 8

I played a bit with the model and noticed that depending on the size of the box's collision item, the box behaves differently, for example:

• With a box of size 1 1 1 (default) the above behavior happens.
• Increasing the size to 2 2 2 the velocity is always read as zero.
• Decreasing the size to 0.5 0.5 0.5 the velocity is "correctly applied".

The behavior is also dependent on the mu friction coefficient under <collision><surface><friction><ode>, so setting it to 0 makes the items move smoothly.

I guess my final question is whether it is intended that when applying a VelocityCmd component the behavior depends on the friction, my understanding was that it would somewhat bypass physics and make sure the model has the commanded velocity, but that doesn't seem to be the case?

[adlarkin]

I guess my final question is whether it is intended that when applying a VelocityCmd component the behavior depends on the friction, my understanding was that it would somewhat bypass physics and make sure the model has the commanded velocity, but that doesn't seem to be the case?

Whether this behavior was intentional or not, I am not sure - however, looking at the physics system, LinearVelocityCmds are applied before the next forward step is taken in the physics engine. Once the forward step is complete, the output linear velocities from the physics engine are used to populate the LinearVelocity component, if it exists. So, it seems like the LinearVelocityCmd almost serves as a "target" velocity input to the physics engine, but then, things like forces are taken into account during the physics forward step, and we get a different result for the output. @azeey can you provide any comments on whether the behavior being described in #966 (comment) is intentional or not?

[azeey]

It looks like the current implementation with DART as the physics engine does not compensate for friction or any other forces when applying a LinearVelocityCmd. It simply sets the current state of the link and when the next step is called, DART computes the next velocity and position starting from the current velocity set via LinearVelocityCmd.

This, unfortunately, does not follow the pattern we have for JointVelocity[Cmd|Reset] where, JointVelocityCmd compensates for joint friction and other effects (within joint force limits) while JointVelocityReset sets the state of the joint and does not compensate for joint friction.

I guess my final question is whether it is intended that when applying a VelocityCmd component the behavior depends on the friction...

So to answer the question, I don't think it was intended. A better name for the current LinearVelocityCmd would have been LinearVelocityReset.

[adlarkin]

Thanks for the explanation! I think that @luca-della-vedova is using TPE though, so if the behavior that was explained in #966 (comment) is when TPE is used, then I assume that we can fix the TPE behavior since it's a plugin inside of ign-physics, right?

[luca-della-vedova]

Thanks for the explanation! I think that @luca-della-vedova is using TPE though, so if the behavior that was explained in #966 (comment) is when TPE is used, then I assume that we can fix the TPE behavior since it's a plugin inside of ign-physics, right?

TPE was only used as a fallback after we observed the issues with friction that made controlling robots through VelocityCmd components a bit tough on dartsim.
I think with the TPE fix and doing "open loop" control in dartsim without checking the current model velocity we are able to cover all our use cases and the DART behavior is not really a bug, just a feature.
We can close this, thanks all for the help!