bullet-featherstone: JointVelocityCmd components remain in effect even after being deleted

[scpeters]

Environment

• OS Version: macOS 14.7-x86_64
• Source or binary build? source build of gz-sim9

Description

• Expected behavior: JointVelocityCmd components are deleted after each timestep since Physics: remove *VelocityCmd at each time step #2228, so they should take effect only for the timesteps when they are added. I modified a test in 1ed3022 to confirm that this is how the dartsim plugin works, and I would expect bullet-featherstone to work this way as well.
• Actual behavior: running the test in bullet-featherstone with b5b47f6 shows that it fails

Steps to reproduce

1. Check out b5b47f6
2. Build from source
3. Run test case: bin/INTEGRATION_physics_system --gtest_filter="*JointsInWorld"

Output

$ bin/INTEGRATION_physics_system --gtest_filter="*JointsInWorld"
Running main() from /Users/scpeters/ws/ionic/src/gz-sim/test/gtest_vendor/src/gtest_main.cc
Note: Google Test filter = *JointsInWorld
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from PhysicsSystemFixture
[ RUN      ] PhysicsSystemFixture.JointsInWorld
(2024-10-23 05:10:58.846) [info] [Server.cc:145] Loading SDF world file[/Users/scpeters/ws/ionic/src/gz-sim/test/worlds/joints_in_world.sdf].
(2024-10-23 05:10:59.052) [info] [SystemManager.cc:54] Serving entity system service on [/entity/system/add]
(2024-10-23 05:10:59.060) [info] [LevelManager.cc:641] Loaded level [default]
(2024-10-23 05:10:59.061) [debug] [ServerConfig.cc:1025] Loading (3) plugins from file [/Users/scpeters/ws/ionic/src/gz-sim/build/test/fake_home/.gz/sim/9/server.config]
(2024-10-23 05:10:59.061) [info] [SimulationRunner.cc:1626] No systems loaded from SDF, loading defaults
(2024-10-23 05:10:59.255) [debug] [Physics.cc:877] Loaded [gz::physics::bullet_featherstone::Plugin] from library [/usr/local/Cellar/gz-physics8/8.0.0/lib/gz-physics-8/engine-plugins/libgz-physics-bullet-featherstone-plugin.dylib]
(2024-10-23 05:10:59.255) [debug] [SystemManager.cc:80] Loaded system [gz::sim::systems::Physics] for entity [1]
(2024-10-23 05:10:59.286) [info] [UserCommands.cc:664] Create service on [/world/joints_in_world/create]
(2024-10-23 05:10:59.286) [info] [UserCommands.cc:671] Remove service on [/world/joints_in_world/remove]
(2024-10-23 05:10:59.286) [info] [UserCommands.cc:678] Pose service on [/world/joints_in_world/set_pose]
(2024-10-23 05:10:59.287) [info] [UserCommands.cc:686] Pose service on [/world/joints_in_world/set_pose_vector]
(2024-10-23 05:10:59.287) [info] [UserCommands.cc:693] Light configuration service on [/world/joints_in_world/light_config]
(2024-10-23 05:10:59.287) [info] [UserCommands.cc:719] Physics service on [/world/joints_in_world/set_physics]
(2024-10-23 05:10:59.287) [info] [UserCommands.cc:727] SphericalCoordinates service on [/world/joints_in_world/set_spherical_coordinates]
(2024-10-23 05:10:59.288) [info] [UserCommands.cc:736] Enable collision service on [/world/joints_in_world/enable_collision]
(2024-10-23 05:10:59.288) [info] [UserCommands.cc:745] Disable collision service on [/world/joints_in_world/disable_collision]
(2024-10-23 05:10:59.288) [info] [UserCommands.cc:754] Material service on [/world/joints_in_world/visual_config]
(2024-10-23 05:10:59.288) [info] [UserCommands.cc:762] Material service on [/world/joints_in_world/wheel_slip]
(2024-10-23 05:10:59.288) [debug] [SystemManager.cc:80] Loaded system [gz::sim::systems::UserCommands] for entity [1]
(2024-10-23 05:10:59.316) [debug] [SystemManager.cc:80] Loaded system [gz::sim::systems::SceneBroadcaster] for entity [1]
(2024-10-23 05:10:59.317) [info] [SimulationRunner.cc:260] Serving world controls on [/world/joints_in_world/control], [/world/joints_in_world/control/state] and [/world/joints_in_world/playback/control]
(2024-10-23 05:10:59.317) [info] [SimulationRunner.cc:286] Serving GUI information on [/world/joints_in_world/gui/info]
(2024-10-23 05:10:59.317) [info] [SimulationRunner.cc:289] World [joints_in_world] initialized with [default_physics] physics profile.
(2024-10-23 05:10:59.317) [info] [SimulationRunner.cc:296] Serving world SDF generation service on [/world/joints_in_world/generate_world_sdf]
(2024-10-23 05:10:59.317) [info] [ServerPrivate.cc:334] Serving world names on [/gazebo/worlds]
(2024-10-23 05:10:59.318) [info] [ServerPrivate.cc:347] Resource path add service on [/gazebo/resource_paths/add].
(2024-10-23 05:10:59.318) [info] [ServerPrivate.cc:360] Resource path get service on [/gazebo/resource_paths/get].
(2024-10-23 05:10:59.318) [info] [ServerPrivate.cc:375] Resource path resolve service on [/gazebo/resource_paths/resolve].
(2024-10-23 05:10:59.318) [info] [ServerPrivate.cc:389] Resource paths published on [/gazebo/resource_paths].
(2024-10-23 05:10:59.318) [info] [ServerPrivate.cc:402] Server control service on [/server_control].
(2024-10-23 05:11:00.775) [info] [SimulationRunner.cc:743] Found no publishers on /stats, adding root stats topic
(2024-10-23 05:11:00.776) [info] [SimulationRunner.cc:777] Found no publishers on /clock, adding root clock topic
(2024-10-23 05:11:00.776) [debug] [SimulationRunner.cc:542] Creating PostUpdate worker threads: 3
(2024-10-23 05:11:00.776) [debug] [SimulationRunner.cc:553] Creating postupdate worker thread (0)
(2024-10-23 05:11:00.777) [debug] [SimulationRunner.cc:553] Creating postupdate worker thread (1)
(2024-10-23 05:11:00.783) [debug] [Physics.cc:1081] Attempting to set physics options, but the phyiscs engine doesn't support feature [CollisionPairMaxContacts]. Options will be ignored.
(2024-10-23 05:11:00.789) [info] [SceneBroadcaster.cc:634] Serving scene information on [/world/joints_in_world/scene/info]
(2024-10-23 05:11:00.790) [info] [SceneBroadcaster.cc:643] Serving graph information on [/world/joints_in_world/scene/graph]
(2024-10-23 05:11:00.790) [info] [SceneBroadcaster.cc:654] Serving full state on [/world/joints_in_world/state]
(2024-10-23 05:11:00.790) [info] [SceneBroadcaster.cc:663] Serving full state (async) on [/world/joints_in_world/state_async]
(2024-10-23 05:11:00.790) [info] [SceneBroadcaster.cc:671] Publishing scene information on [/world/joints_in_world/scene/info]
(2024-10-23 05:11:00.790) [info] [SceneBroadcaster.cc:680] Publishing entity deletions on [/world/joints_in_world/scene/deletion]
(2024-10-23 05:11:00.790) [info] [SceneBroadcaster.cc:689] Publishing state changes on [/world/joints_in_world/state]
(2024-10-23 05:11:00.791) [info] [SceneBroadcaster.cc:700] Publishing pose messages on [/world/joints_in_world/pose/info]
(2024-10-23 05:11:00.791) [info] [SceneBroadcaster.cc:711] Publishing dynamic pose messages on [/world/joints_in_world/dynamic_pose/info]
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
...
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2927: Failure
The difference between (*jointPos)[0] and 1.57079632679489661923132169163975144 is 0.049729867777552172, which exceeds 1e-2, where
(*jointPos)[0] evaluates to 1.6205261945724487,
1.57079632679489661923132169163975144 evaluates to 1.5707963267948966, and
1e-2 evaluates to 0.01.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2927: Failure
The difference between (*jointPos)[0] and 1.57079632679489661923132169163975144 is 0.049829526743616626, which exceeds 1e-2, where
(*jointPos)[0] evaluates to 1.6206258535385132,
1.57079632679489661923132169163975144 evaluates to 1.5707963267948966, and
1e-2 evaluates to 0.01.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2927: Failure
The difference between (*jointPos)[0] and 1.57079632679489661923132169163975144 is 0.049929185709681079, which exceeds 1e-2, where
(*jointPos)[0] evaluates to 1.6207255125045776,
1.57079632679489661923132169163975144 evaluates to 1.5707963267948966, and
1e-2 evaluates to 0.01.
/Users/scpeters/ws/ionic/src/gz-sim/test/integration/physics_system.cc:2928: Failure
The difference between (*jointVel)[0] and 0.0 is 0.09960000216960907, which exceeds 1e-6, where
(*jointVel)[0] evaluates to 0.09960000216960907,
0.0 evaluates to 0, and
1e-6 evaluates to 9.9999999999999995e-07.
(2024-10-23 05:11:00.890) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (0)
(2024-10-23 05:11:00.890) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (1)
[  FAILED  ] PhysicsSystemFixture.JointsInWorld (2195 ms)
[----------] 1 test from PhysicsSystemFixture (2195 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (2195 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] PhysicsSystemFixture.JointsInWorld

 1 FAILED TEST