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

Fix Joint Position Controller Behaviour Described in #1997 #2001

Merged
merged 15 commits into from
Aug 3, 2023

Conversation

arjo129
Copy link
Contributor

@arjo129 arjo129 commented May 31, 2023

🦟 Bug fix

Fixes #1997

Summary

This commit addresses #1997

There are several issues at play. First the target velocity calculation was wrong as described in #1997. Secondly, even if we corrected that there was still incorrect behavior. This is due to the fact that we use the PID's CmdMax to determine what the maximum velocity for the joint is. However, in the event a user does not set a <cmd_max> this defaults to zero and the joint does not move. Finally this PR updates the tests. Previously our tests were only testing the case where the position command was well above the position's maximum velocity, hence it would slide into position. This PR introduces a test where we snap the position of the joint into place instead.

Checklist

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

This commit addresses #1997

There are several issues at play. First the target velcity calculation
was wrong as described in #1997. Secondly, even if we corrected that
there was still incorrect behaviour. This is due to the fact that we use
the PID's CmdMax to determine what the maximum velocity for the joint
is. However, in the event a user does not set a `<cmd_max>` this
defaults to zero and the joint does not move. Finally this PR updates
the tests. Previously our tests were only testing the case where the
position command was well above the position's maximum velocity, hence
it would slide into position. This PR introduces a test where we snap
the position of the joint into place instead.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@codecov
Copy link

codecov bot commented May 31, 2023

Codecov Report

Merging #2001 (d80b7b3) into ign-gazebo6 (4ce01ea) will decrease coverage by 0.02%.
Report is 1 commits behind head on ign-gazebo6.
The diff coverage is 83.33%.

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

@@               Coverage Diff               @@
##           ign-gazebo6    #2001      +/-   ##
===============================================
- Coverage        65.35%   65.34%   -0.02%     
===============================================
  Files              327      327              
  Lines            27002    27086      +84     
===============================================
+ Hits             17648    17698      +50     
- Misses            9354     9388      +34     
Files Changed Coverage Δ
include/gz/sim/EntityComponentManager.hh 100.00% <ø> (ø)
include/gz/sim/EventManager.hh 75.75% <66.66%> (-3.41%) ⬇️
src/systems/sensors/Sensors.cc 65.65% <77.64%> (-0.09%) ⬇️
src/EntityComponentManager.cc 89.71% <97.22%> (+0.32%) ⬆️
...int_position_controller/JointPositionController.cc 73.75% <100.00%> (+0.18%) ⬆️
src/systems/scene_broadcaster/SceneBroadcaster.cc 92.20% <100.00%> (+0.01%) ⬆️

... and 2 files with indirect coverage changes

arjo129 added 2 commits May 31, 2023 08:32
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129 arjo129 requested a review from ahcorde May 31, 2023 08:39
arjo129 added 3 commits June 1, 2023 02:35
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@azeey
Copy link
Contributor

azeey commented Jun 1, 2023

Do we need to consider backward compatibility? How will this change affect models that currently use this plugin?
Just saw your comment #1997 (comment)

@jrutgeer
Copy link
Contributor

jrutgeer commented Jun 6, 2023

It still doesn't work for me, but based on the following I conclude there must also be another issue:

What I did:

  1. Copy paste the proposed changes into my local JointPositionController.cc (as I am on Garden, not Fortress).
  2. Added a bool new_command that is set upon reception of a new position command, and following debug output:
    if (new_command) std::cout << "targetVel: " << targetVel << ", error: " << error << ", dt: " << dt << std::endl;
  3. Change the SDF to listen on topic /rotor_cmd
  4. Send following messages consecutively:
$ gz topic -t "/rotor_cmd" -m gz.msgs.Double -p "data: 1.0"
$ gz topic -t "/rotor_cmd" -m gz.msgs.Double -p "data: 1.0"
$ gz topic -t "/rotor_cmd" -m gz.msgs.Double -p "data: 100.0"
$ gz topic -t "/rotor_cmd" -m gz.msgs.Double -p "data: 100.0"

The object visual does not move in the GUI (for either of above messages).
However the debug output is as follows:

targetVel: 1000, error: -1, dt: 0.001
targetVel: -0, error: 0, dt: 0.001
targetVel: 99000, error: -99, dt: 0.001
targetVel: -0, error: 0, dt: 0.001

So even though the object does not move in the GUI, the error is zero on each 2nd issue of an identical position command, i.e. the position must be reached, even though it is not reflected in the GUI.

If you reduce the factor, e.g. targetVel = -error/(dt*2); instead of targetVel = -error/dt; then the object does move in the GUI (but it will obviously not reach the position in one timestep).

Rather a bug in gz-rendering then??

@arjo129
Copy link
Contributor Author

arjo129 commented Jun 7, 2023

Any chance you can share the specific sdf? It could be an issue of angle wrapping also. The joint position controller does not wrap angles.

I can reproduce your error with the example file attached.
example_velocity.sdf.gz

@arjo129
Copy link
Contributor Author

arjo129 commented Jun 7, 2023

Ok narrowed it down to physics. Seems like setting a JointVelocityCmd does not affect the Link poisition but updates the joint position. Thus we think the position is correct, but in reality the Link poisition should have moved. Ironically JointForceCmd works as it updates the position of the link. Could this be an error in Dart upstream?

Update: Dart is updating the pose, it doesn't seem to get serialized after this line. Perhaps it is a scene broadcaster bug? Or an ecs bug?:

// to the parent. Same for links inside nested models.

@arjo129
Copy link
Contributor Author

arjo129 commented Jun 7, 2023

OK figured it out this is really hairy (The changes in this PR are still necessary for the snap to functionality to work). The remaining problem is that the GUI goes out of sync when we don't regularly update a component marked as PeriodicChange. Since using the velocity to set position is only done once, we would have to ensure that the pose is accumulated.

Seems like the pose updates get dropped because we use ComponentState::PeriodicChange here:

ComponentState::PeriodicChange);

Using OneTimeChange solves the issue where the GUI goes out of sync, however that is not a long term solution. Alternatively our ECS should keep track of which "changes exist but haven't been transmitted yet" (this seems better but harder). There are several options (in increasing order of difficulty):

  1. Modify Physics.cc such that when a velocity command is sent we use the OneTimeChange instead of PeriodicChange (likely has performance implications).
  2. Alternatively, modify this plugin to use JointPositionResetCmd. This however has problems as it will require that all indices of the joint have a position set.
  3. Introduce a new message and component JointPositionChange that changes only the specified index of the joint.
  4. Modify the ECM and state manager such that changes are accumulated across updates so when a change is transmitted it always contains the last set of changes.

I'm tempted to go with 2 or 3 as that seems the cleanest to me. However it might be worth considering 4 in some future release.

@jrutgeer
Copy link
Contributor

jrutgeer commented Jun 7, 2023

@arjo129 Thank you so much for looking into this!

The proposed changes are beyond my level of comprehension to provide relevant feedback I'm afraid...

However, while digging through the code trying to understand it, I found following comments regarding what seems a similar issue:

// log files may be recorded at lower rate than sim time step. So in
// playback mode, the scene broadcaster may not see any periodic
// changed states here since it no longer happens every iteration.
// As the result, no state changes are published to be GUI, causing
// visuals in the GUI scene to miss updates. The visuals are only
// updated if by some timing coincidence that log playback updates
// the ECM at the same iteration as when the scene broadcaster is going
// to publish perioidc changes here.
// To work around the issue, we force the scene broadcaster
// to publish states at an offcycle iteration the next time it sees
// periodic changes.
auto playbackComp =
_manager.Component<components::LogPlaybackStatistics>(
this->dataPtr->worldEntity);
if (playbackComp)
{
this->dataPtr->pubPeriodicChanges = true;
}
// this creates an empty state in the msg even there are no periodic
// changed components - done to preseve existing behavior.
// we may be able to remove this in the future and update tests
this->dataPtr->stepMsg.mutable_state();
}

So maybe something similar could be a solution?

@jrutgeer
Copy link
Contributor

jrutgeer commented Jun 7, 2023

@arjo129

I haven't been able to fully test it yet, but it seems that this might also be a possible solution:

  • Make JointPositionController inherit also from ISystemUpdate,
  • Add an update() similar to this:
void JointPositionController::Update(const gz::sim::UpdateInfo &_info,
    gz::sim::EntityComponentManager &_ecm)
{
  (void) _info;
    
  if (new_command)
  {
    auto jointPoseComp = _ecm.Component<components::Pose>(
       this->dataPtr->jointEntities[0]);

    if (jointPoseComp)
        _ecm.SetChanged(this->dataPtr->jointEntities[0], components::Pose::typeId,
          ComponentState::OneTimeChange);
  }
}

Not sure of possible drawbacks though...

@azeey
Copy link
Contributor

azeey commented Jun 7, 2023

I was also able to reproduce this and I agree that it's an issue with GUI going out of sync with the server. The list of components that are marked PeriodicChange gets cleared in every iteration, but it's quite likely that the SceneBroadcaster misses some of them because it only publishes at 60Hz. However, this is not limited to cases where JointVelocityCmd is used. This can happen with any sudden pose change, e.g., a force that displaces an object but does it in such a way that it comes to a stop within a few iterations after being displaced. So, I don't think the solution to this should live in JointPositionController or Physics. I think SceneBroadcaster should accumulate all the components that have periodic changes and publish them at the next scheduled publication. Having some sort of functionality in the ECM that facilitates this would be great, but as @arjo129 mentioned, that would be difficult since each system will have different timing requirements (i.e, the SceneBroadcaster needs to clear the PeriodicChange components at 60Hz but other systems might have different frequencies).

@jrutgeer
Copy link
Contributor

jrutgeer commented Jun 8, 2023

@azeey Do I understand it correctly?

  • In each simulation timestep (e.g. 1000Hz) the SceneBroadcaster does always publish components that are marked OneTimeChange,
  • In each 'SceneBroadcaster timestep' (60Hz) the SceneBroadcaster publishes all components that are (still) marked PeriodicChange,

And:

  1. Introduce a new message and component JointPositionChange that changes only the specified index of the joint.

This means e.g.:

  • In each simulation timestep (e.g. 1000Hz) the SceneBroadcaster will always publish components that are marked OneTimeChange, and will add a component "to_be_broadcasted" to each entity that has PeriodicChange components,
  • The PeriodicChange gets cleared in hte next simulation iteration, but the "to_be_broadcasted" remains,
  • In each 'SceneBroadcaster timestep' (60Hz) the SceneBroadcaster will publish all components that have a PeriodicChange OR "to_be_broadcasted" component and will clear the "to_be_broadcasted" component from the corresponding entities.

Correct?

@azeey
Copy link
Contributor

azeey commented Jun 8, 2023

@azeey Do I understand it correctly?

  • In each simulation timestep (e.g. 1000Hz) the SceneBroadcaster does always publish components that are marked OneTimeChange,
  • In each 'SceneBroadcaster timestep' (60Hz) the SceneBroadcaster publishes all components that are (still) marked PeriodicChange,

And:

  1. Introduce a new message and component JointPositionChange that changes only the specified index of the joint.

This means e.g.:

  • In each simulation timestep (e.g. 1000Hz) the SceneBroadcaster will always publish components that are marked OneTimeChange, and will add a component "to_be_broadcasted" to each entity that has PeriodicChange components,
  • The PeriodicChange gets cleared in hte next simulation iteration, but the "to_be_broadcasted" remains,
  • In each 'SceneBroadcaster timestep' (60Hz) the SceneBroadcaster will publish all components that have a PeriodicChange OR "to_be_broadcasted" component and will clear the "to_be_broadcasted" component from the corresponding entities.

Correct?

Yes, I think we're on the same page. The only thing I'd say is that the PeriodicChange and OneTimeChange "labels" are applied to components, not entities, so it doesn't make sense to say "the SceneBroadcaster will publish all components that have a PeriodicChange OR "to_be_broadcasted" component" because the component cannot have a to_be_broadcasted component (i.e, only entities have components). So my suggested implementation is for the SceneBroadcaster itself to keep track of the PeriodicChange components (and associated entities) that would currently be missed and mark them to_be_broadcasted internally somehow.

@arjo129
Copy link
Contributor Author

arjo129 commented Jun 9, 2023

Ill have a pr for this ready soon.

arjo129 added a commit that referenced this pull request Jun 9, 2023
This commit proposes a change to the scene broadcaster which enables
tracking of all components with periodic changes. This way if a
component has a periodic change the scene broadcaster does not miss it.

For more info see this discussion #2001 (comment) where @azeey proposes this solution.

This commit is WIP and I still need to handle entity/component removal
and add tests.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Copy link
Contributor Author

arjo129 commented Jun 9, 2023

@jrutgeer : You can try this branch #2010 where I'm hashing out the SceneBroadcaster implementation (It's a WIP).

@jrutgeer
Copy link
Contributor

jrutgeer commented Jun 9, 2023

@azeey
I see, thanks for the clarification. Yet, still to me it feels conceptually strange to implement a 'local cache' of components into the scene broadcaster (and each possible other system that also deals with PeriodicChange components at a reduced frequency). This means reproducing functionality (adding / removing components etc) in each of those systems, which already exits in the ECM.

Given PeriodicChange is kind of a 'tag' of a component, I would rather assume some way for each system to add their own tags to the components (e.g. scene broadcaster would tag each PeriodicChange component with tag broadcast_needed) and then in each "60Hz step" iterate over all components that have that specific tag.

Anyway, I guess many ways lead to Rome... :-)

@arjo129

You can try this branch #2010 where I'm hashing out the SceneBroadcaster implementation (It's a WIP).

Is there some git wizzardry command to merge this diff into my local Gazebo Garden branch? I tried several fetch/merge/rebase combinations but I don't succeed. Yet there must be a better way than manual copy-paste...
See also my related question on robotics stack exchange.

@arjo129
Copy link
Contributor Author

arjo129 commented Jun 9, 2023

@jrutgeer I believe git cherry-pick should work: https://git-scm.com/docs/git-cherry-pick

@azeey
Copy link
Contributor

azeey commented Jun 9, 2023

Yet, still to me it feels conceptually strange to implement a 'local cache' of components into the scene broadcaster (and each possible other system that also deals with PeriodicChange components at a reduced frequency). This means reproducing functionality (adding / removing components etc) in each of those systems, which already exits in the ECM.

I agree this is not ideal, but we don't have a generalized mechanism to do this and as far as I know, only the SceneBroadcaster system suffers from this problem currently. Implementing the generalized mechanism would be a lot more complicated and probably not worth the effort right now.

@jrutgeer
Copy link
Contributor

@arjo129

I believe git cherry-pick should work

I didn't look into git cherry-pick, but, as @azeey kindly pointed out in his answer on RSE: there are forward port instructions here ("Porting changes across branches").

For reference: the procedure is:

cd gz-sim
git checkout arjo/fix/1997          # get the branch you want to merge from
git pull
git checkout gz-sim7                # get the branch you want to merge into
git pull
git checkout -b merge_fix           # create new branch to merge into
git merge arjo/fix/1997             # merge
# manually fix merge conflicts and commit.

To merge also #2010, another iteration is needed:

git checkout arjo/fix/scene_broadcaster_track_changes    # get the branch you want to merge from
git pull
git checkout merge_fix                                   # switch to the existing merge branch
git merge arjo/fix/scene_broadcaster_track_changes       # merge

arjo129 and others added 4 commits July 18, 2023 06:55
* Tracks periodic changes in scene broadcaster

This commit proposes a change to the scene broadcaster which enables
tracking of all components with periodic changes. This way if a
component has a periodic change the scene broadcaster does not miss it.

For more info see this discussion #2001 (comment) where @azeey proposes this solution.

This commit is WIP and I still need to handle entity/component removal
and add tests.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Rework changes

* Removes clone made of BaseComponent.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* More reworks of added APIs to ECM.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fix tests

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Add ECM related tests.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address spelling issue

Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>

* Get rid of TODO

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

---------

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Copy link
Contributor

@azeey azeey left a comment

Choose a reason for hiding this comment

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

Looks pretty good. My main concern is with naming and documentation.

include/gz/sim/EntityComponentManager.hh Outdated Show resolved Hide resolved
include/gz/sim/EntityComponentManager.hh Outdated Show resolved Hide resolved
include/gz/sim/EntityComponentManager.hh Outdated Show resolved Hide resolved
src/EntityComponentManager.cc Show resolved Hide resolved
src/EntityComponentManager.cc Show resolved Hide resolved
arjo129 and others added 3 commits July 26, 2023 10:19
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Copy link
Contributor Author

arjo129 commented Jul 28, 2023

@azeey I've addressed the renaming. The other issues highlighted have certain The SceneBroadcaster test seems flaky but I think it does not have much to do with this PR.

@azeey azeey added the beta Targeting beta release of upcoming collection label Jul 31, 2023
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@azeey
Copy link
Contributor

azeey commented Aug 1, 2023

@osrf-jenkins run tests please

@azeey
Copy link
Contributor

azeey commented Aug 2, 2023

Having some infrastructure issues on Jenkins. Here's a manual Ubuntu build: Build Status

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Copy link
Contributor

@azeey azeey left a comment

Choose a reason for hiding this comment

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

Looks great! Thanks for iterating.

@azeey
Copy link
Contributor

azeey commented Aug 2, 2023

@ahcorde okay if I merge this?

@arjo129 arjo129 enabled auto-merge (squash) August 3, 2023 05:15
@arjo129 arjo129 merged commit 7713f2b into ign-gazebo6 Aug 3, 2023
@arjo129 arjo129 deleted the arjo/fix/1997 branch August 3, 2023 16:38
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
beta Targeting beta release of upcoming collection 🏯 fortress Ignition Fortress
Projects
Archived in project
Development

Successfully merging this pull request may close these issues.

JointPositionController in ABS mode has first-order behavior rather than setting the position
4 participants