Clearing pending commands - unexpected behavior

[unjambonakap]

Hi,

Issue summary:

• Set LinearVelocityCmd to non zero vector (for instance to give the system an initial velocity)
• This velocity is used on first integration step
• LinearVelocityCmd gets cleared to zero vector
• The model stops moving on all subsequent steps (while we don't ecm.RemoveComponent(...))

When specify attaching a command to an entity (such as JointForceCmd, AngularVelocityCmd), these commands get cleared after a physic step (physics.cc)

This clearing has, I'd say, an unexpected behavior for some commands.
Most notably, the velocity commands (angular and linear) get set to the zero vector. This zero vector is then used on later steps to overwrite the velocities.

I suppose the clearing of commands after an integration step means that these commands are considered as impulses - meaning a command should only alter one step and then be no-op.
While clearing a force to the zero vector achieves that, doing the same for the velocity is not correct.

Possibles solutions:

1. deleting the commands instead of clearing them to zero
2. modifying the content of the velocity commands to have an optional

I'd favor (1) but I've just started using gazebo so I don't know much :)

Edit: I could do the PR once we've settled on a direction

Thanks

[azeey]

While clearing a force to the zero vector achieves that, doing the same for the velocity is not correct.

Ah, yes, I think clearing to zero only makes sense for force components.

deleting the commands instead of clearing them to zero

I'd favor this too. I think in the past we had performance issues with repeatedly creating and deleting components, but I believe that has been fixed as of #856. The only caveat is I think this would be considered a behavior change since a system that assumes that keeps updating a velocity command now needs to recreate the component in every step, so at best, we'd have to target a PR against main, but we might have to live with the current behavior if too many systems depend on the old behavior.

[iche033]

Looking throughout the codebase, there are a few systems and tests that rely on the current behavior so it might be a good option to go with the approach of creating a new vel cmd component for the different behavior. For the Link API, We can also add an optional arg to indicate whether the vel cmd should persist or not, e.g. SetLinearVelocity(EntityComponentManager &_ecm, const math::Vector3d &_vel, bool _persist = false)

[unjambonakap]

Could you point to places that rely on this behavior? I'm interested in seeing a situation where the current behavior is useful because I really can't conceive a situation where it makes sense (nor reconcile it semantically to the function's name)

We have 3 behavior:
logical persist: link.SetV(v, persist=true) -> link.v = v, integrate(1), link.v = v, ...
logical dirac: link.SetV(v, persist=false) -> link.v = v, integrate(1) ...
actual behavior: link.SetV(v) -> link.v = v, integrate(1) ,link.v = 0, integrate(1), link.v = 0, ...

I agree that a _persist flag would be a nice feature. A new command for it is fine though the current command's behavior needs to be changed (either through a second new command or through a delete of the component).

[iche033]

I noticed that most of the systems and tests inside gz-sim had to send the cmd continuously, e.g. velocity_control system and a test helper class. The behavior change here is that if one of our users had code to move a model for a fixed period of time, e.g. by sending vel cmds for 5 seconds then expect the model to stop moving once they stop sending the cmd, the new proposed change would break this behavior.

[unjambonakap]

Fair point for user code (though I hope they do not do that :) ).
Those systems sending the command continuously wouldn't be affected by this, so that's good.

How do you want to proceed: should I stop bothering you and leave you people to it (I don't know much about the organization, if you're full time on gazebo or not) or we settle on a direction and I write a PR?
That direction could be to introduce a sidekick to LinearVelocityCmd with a persisted and dirac version with the appropriate helpers

[iche033]

PRs are welcome :) I think we just need to agree on the design and what's to be implemented. @azeey what do you think adding a new component or do you have other ideas?

[azeey]

Those systems sending the command continuously wouldn't be affected by this, so that's good.

I don't think that's quite right because those systems assume the component is there and just update the value. If we remove the component, those systems would have to be updated to recreate the component in every time step.

We could create new components (eg. *VelocityImpulseCmd and *VelocityPersistedCmd) and leave the current components unchanged so we are backward compatible, but how would these new components interact with each other and with the existing *VelocityCmd components. How should the Physics system prioritize the components? And how would multiple systems that each use one of these components coexist? I don't have a good answer for these. It seems a lot cleaner and easier to reason about to have one set of *VelocityCmd components. Systems that need persisted velocities can just create and set the component in each time step. Systems that need the the robot to stop would just set the component to 0.

So my proposal would be to change the current behavior in the main branch and have the Physics system remove *VelocityCmd components. Now that we have EntityComponentManager::SetComponentData, which creates a component if it doesn't exist, it wouldn't be a difficult transition for users to make. For multiple system interactions, perhaps we can create a EntityComponentManager::AddComponentData, so they don't clobber each other's data.

BTW, I also see the same problem with JointVelocityCmd, so the decision we make here should be applied for all *VelocityCmd components.

[scpeters]

It seems a lot cleaner and easier to reason about to have one set of *VelocityCmd components. Systems that need persisted velocities can just create and set the component in each time step. Systems that need the the robot to stop would just set the component to 0.

So my proposal would be to change the current behavior in the main branch and have the Physics system remove *VelocityCmd components. Now that we have EntityComponentManager::SetComponentData, which creates a component if it doesn't exist, it wouldn't be a difficult transition for users to make. For multiple system interactions, perhaps we can create a EntityComponentManager::AddComponentData, so they don't clobber each other's data.

BTW, I also see the same problem with JointVelocityCmd, so the decision we make here should be applied for all *VelocityCmd components.

I've started work on this suggestion in #2228

[scpeters]

all *VelocityCmd components are now removed at each timestep as of #2228, so I will close this issue. Please reopen if there is still something remaining to be done