Get joint torque value from TransmittedJointTorque returned by gz-sim #86

xela-95 · 2024-02-14T11:40:30Z

Closes #81

This PR aims at computing the value of the joint torque from the corresponding JointTransmittedWrench returned by the gz-sim simulator. At the moment this is supported only for revolute joints.

codecov · 2024-02-14T11:44:18Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (5f4815a) 83.31% compared to head (efe775d) 61.25%.

Additional details and impacted files

@@             Coverage Diff             @@
##             main      #86       +/-   ##
===========================================
- Coverage   83.31%   61.25%   -22.07%     
===========================================
  Files          16       20        +4     
  Lines        1001     1768      +767     
===========================================
+ Hits          834     1083      +249     
- Misses        167      685      +518

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

xela-95 · 2024-02-16T13:43:06Z

@traversaro with 599fab0 I started having a working (not passing) unit test. It was quite hard to understand how the fixture works but basically it calls the callbacks (OnConfigure, OnPreUpdate, OnPostUpdate) after having called the corresponding methods for the plugin, so it's easy to obtain data directly from the device without having to duplicate configuration code already done in the plugin classes.

Here the logs of the test launched with $ ctest -R ControlBoardTest --verbose for 10 iterations:

Details


UpdateCTestConfiguration  from :/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
Parse Config file:/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
UpdateCTestConfiguration  from :/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
Parse Config file:/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
Test project /home/acroci/repos/gz-sim-yarp-plugins/build
Constructing a list of tests
Done constructing a list of tests
Updating test list for fixtures
Added 0 tests to meet fixture requirements
Checking test dependency graph...
Checking test dependency graph end
test 6
    Start 6: ControlBoardTest

6: Test command: /home/acroci/repos/gz-sim-yarp-plugins/build/bin/ControlBoardTest
6: Working Directory: /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard
6: Environment variables: 
6:  GZ_SIM_SYSTEM_PLUGIN_PATH=/home/acroci/repos/gz-sim-yarp-plugins/build/lib
6: Test timeout computed to be: 1500
6: Running main() from /home/acroci/repos/gz-sim-yarp-plugins/build/_deps/googletest-src/googletest/src/gtest_main.cc
6: [==========] Running 1 test from 1 test suite.
6: [----------] Global test environment set-up.
6: [----------] 1 test from ControlBoardTest
6: [ RUN      ] ControlBoardTest.GetTorqueWithPendulumJointRelativeToParentLink
6: [Msg] Loading SDF world file[/home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/pendulum_joint_relative_to_parent_link.sdf].
6: [Msg] Serving entity system service on [/entity/system/add]
6: [INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device  loaded from yarpConfigurationFile :  ../../../tests/controlboard/conf/gazebo_controlboard.ini 
6: [DEBUG] gz-sim-yarp-controlboard-system : robot scoped name:  model/single_pendulum
6: [DEBUG] gz-sim-yarp-controlboard-system : device scoped name:  model/single_pendulum/controlboard_plugin_device
6: [DEBUG] gz-sim-yarp-controlboard-system: initialConfiguration:  0.0 0.0
6: [DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (GAZEBO_PIDS (Pid0 1000.0 2.0 0.100000000000000005551 9999 9999 9 9)) (GAZEBO_VELOCITY_PIDS (Pid0 500.0 2.0 0.100000000000000005551 9999 9999 9 9)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (initialConfiguration "0.0 0.0") (jointNames upper_joint) (name "model/single_pendulum/controlboard_plugin_device") (robotScopedName "model/single_pendulum") (yarpDeviceName controlboard_plugin_device)
6: [INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
6: [INFO] Found 1 joints from the plugin configuration.
6: [INFO] Found 1 joints from the model description.
6: [INFO] Joint  upper_joint  added to the control board data.
6: [INFO] Registered YARP device with instance name: model/single_pendulum/controlboard_plugin_device
6: [Dbg] [SystemManager.cc:74] Loaded system [gzyarp::ControlBoard] for entity [8]
6: [Msg] Loaded level [3]
6: [Msg] No systems loaded from SDF, loading defaults
6: [Dbg] [ServerConfig.cc:1062] Loaded (3) plugins from file [/home/acroci/.gz/sim/7/server.config]
6: [Dbg] [Physics.cc:880] Loaded [gz::physics::dartsim::Plugin] from library [/home/acroci/mambaforge/envs/gz7/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so]
6: [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::Physics] for entity [1]
6: [Msg] Create service on [/world/turorial_controlboard/create]
6: [Msg] Remove service on [/world/turorial_controlboard/remove]
6: [Msg] Pose service on [/world/turorial_controlboard/set_pose]
6: [Msg] Pose service on [/world/turorial_controlboard/set_pose_vector]
6: [Msg] Light configuration service on [/world/turorial_controlboard/light_config]
6: [Msg] Physics service on [/world/turorial_controlboard/set_physics]
6: [Msg] SphericalCoordinates service on [/world/turorial_controlboard/set_spherical_coordinates]
6: [Msg] Enable collision service on [/world/turorial_controlboard/enable_collision]
6: [Msg] Disable collision service on [/world/turorial_controlboard/disable_collision]
6: [Msg] Material service on [/world/turorial_controlboard/visual_config]
6: [Msg] Material service on [/world/turorial_controlboard/wheel_slip]
6: [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::UserCommands] for entity [1]
6: [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::SceneBroadcaster] for entity [1]
6: [Msg] Serving world controls on [/world/turorial_controlboard/control], [/world/turorial_controlboard/control/state] and [/world/turorial_controlboard/playback/control]
6: [Msg] Serving GUI information on [/world/turorial_controlboard/gui/info]
6: [Msg] World [turorial_controlboard] initialized with [default_physics] physics profile.
6: [Msg] Serving world SDF generation service on [/world/turorial_controlboard/generate_world_sdf]
6: [Msg] Serving world names on [/gazebo/worlds]
6: [Msg] Resource path add service on [/gazebo/resource_paths/add].
6: [Msg] Resource path get service on [/gazebo/resource_paths/get].
6: [Msg] Resource path resolve service on [/gazebo/resource_paths/resolve].
6: [Msg] Resource paths published on [/gazebo/resource_paths].
6: [Msg] Server control service on [/server_control].
6: ========== configuring test
6: ========== test configured
6: [Msg] Found no publishers on /stats, adding root stats topic
6: [Msg] Found no publishers on /clock, adding root clock topic
6: [Dbg] [SimulationRunner.cc:503] Creating PostUpdate worker threads: 4
6: [Dbg] [SimulationRunner.cc:514] Creating postupdate worker thread (0)
6: [Dbg] [SimulationRunner.cc:514] Creating postupdate worker thread (1)
6: [Dbg] [SimulationRunner.cc:514] Creating postupdate worker thread (2)
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.72667
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.7266711729438957, which exceeds 1e-3, where
6: joint_torque evaluates to 1.0000000000000004,
6: expected_joint_torque evaluates to 3.7266711729438962, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: [Msg] Serving scene information on [/world/turorial_controlboard/scene/info]
6: [Msg] Serving graph information on [/world/turorial_controlboard/scene/graph]
6: [Msg] Serving full state on [/world/turorial_controlboard/state]
6: [Msg] Serving full state (async) on [/world/turorial_controlboard/state_async]
6: [Msg] Publishing scene information on [/world/turorial_controlboard/scene/info]
6: [Msg] Publishing entity deletions on [/world/turorial_controlboard/scene/deletion]
6: [Msg] Publishing state changes on [/world/turorial_controlboard/state]
6: [Msg] Publishing pose messages on [/world/turorial_controlboard/pose/info]
6: [Msg] Publishing dynamic pose messages on [/world/turorial_controlboard/dynamic_pose/info]
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599995830232632, which exceeds 1e-3, where
6: joint_torque evaluates to 1.0000000000000009,
6: expected_joint_torque evaluates to 3.8599995830232641, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599999987607081, which exceeds 1e-3, where
6: joint_torque evaluates to 0.99999999999999978,
6: expected_joint_torque evaluates to 3.8599999987607081, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599999971054046, which exceeds 1e-3, where
6: joint_torque evaluates to 1.0000000000000002,
6: expected_joint_torque evaluates to 3.8599999971054046, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599999941244656, which exceeds 1e-3, where
6: joint_torque evaluates to 1.0000000000000002,
6: expected_joint_torque evaluates to 3.8599999941244656, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599999892427928, which exceeds 1e-3, where
6: joint_torque evaluates to 1,
6: expected_joint_torque evaluates to 3.8599999892427928, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599999817791497, which exceeds 1e-3, where
6: joint_torque evaluates to 0.99999999999999978,
6: expected_joint_torque evaluates to 3.8599999817791497, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.859999970947368, which exceeds 1e-3, where
6: joint_torque evaluates to 0.99999999999999933,
6: expected_joint_torque evaluates to 3.8599999709473671, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.85999995585634, which exceeds 1e-3, where
6: joint_torque evaluates to 0.99999999999999978,
6: expected_joint_torque evaluates to 3.85999995585634, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 1 - expected: 3.86
6: /home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc:133: Failure
6: The difference between joint_torque and expected_joint_torque is 2.8599999355100252, which exceeds 1e-3, where
6: joint_torque evaluates to 0.99999999999999978,
6: expected_joint_torque evaluates to 3.8599999355100247, and
6: 1e-3 evaluates to 0.001.
6: ========== Test PostUpdate done
6: [Dbg] [SimulationRunner.cc:530] [Dbg] [SimulationRunner.cc:530] [Dbg] [SimulationRunner.cc:530] Exiting postupdate worker thread (1)
6: Exiting postupdate worker thread (0)
6: Exiting postupdate worker thread (2)
6: [  FAILED  ] ControlBoardTest.GetTorqueWithPendulumJointRelativeToParentLink (2026 ms)
6: [----------] 1 test from ControlBoardTest (2026 ms total)
6: 
6: [----------] Global test environment tear-down
6: [==========] 1 test from 1 test suite ran. (2026 ms total)
6: [  PASSED  ] 0 tests.
6: [  FAILED  ] 1 test, listed below:
6: [  FAILED  ] ControlBoardTest.GetTorqueWithPendulumJointRelativeToParentLink
6: 
6:  1 FAILED TEST
1/1 Test #6: ControlBoardTest .................***Failed    2.07 sec

0% tests passed, 1 tests failed out of 1

Total Test time (real) =   2.07 sec

The following tests FAILED:
          6 - ControlBoardTest (Failed)
Errors while running CTest
Output from these tests are in: /home/acroci/repos/gz-sim-yarp-plugins/build/Testing/Temporary/LastTest.log
Use "--rerun-failed --output-on-failure" to re-run the failed cases verbosely.

The mismatch between joint and link quantities is due to the fact that I can access from the Link API link quantities expressed in world frame. I have to understand how to change the reference frame.

xela-95 · 2024-02-16T17:20:11Z

With the last commit I introduced some modifications to the SDF model used for the test:

The starting position of the pendulum is at the stable equilbria
I've computed manually the inertia tensor of the pendulum link in order to have simulation results comparable to the eq. of motion

I've modified also the unit test code: now it uses the correct inertia for the link and while I'm trying to understand how to get the joint acceleration, I'm using the finite differences computations of the joint velocity. An alternative could be to use the link angular acceleration.

The test is now passing, but is numerically fragile due to the finite difference computation of the joint acceleration. I've posted questions on how to get the data from Gazebo in the OSRF Discord channel and on Robotics Stack Exchange).

Now I can focus on testing different models (e.g. when the joint frame is defined wrt the child link or arbitrary frames) and check whether my code to compute the torque projected on the joint axis works.

Test Logs

Parse Config file:/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
UpdateCTestConfiguration  from :/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
Parse Config file:/home/acroci/repos/gz-sim-yarp-plugins/build/DartConfiguration.tcl
Test project /home/acroci/repos/gz-sim-yarp-plugins/build
Constructing a list of tests
Done constructing a list of tests
Updating test list for fixtures
Added 0 tests to meet fixture requirements
Checking test dependency graph...
Checking test dependency graph end
test 6
    Start 6: ControlBoardTest

6: Test command: /home/acroci/repos/gz-sim-yarp-plugins/build/bin/ControlBoardTest
6: Working Directory: /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard
6: Environment variables: 
6:  GZ_SIM_SYSTEM_PLUGIN_PATH=/home/acroci/repos/gz-sim-yarp-plugins/build/lib
6: Test timeout computed to be: 1500
6: Running main() from /home/acroci/repos/gz-sim-yarp-plugins/build/_deps/googletest-src/googletest/src/gtest_main.cc
6: [==========] Running 1 test from 1 test suite.
6: [----------] Global test environment set-up.
6: [----------] 1 test from ControlBoardTest
6: [ RUN      ] ControlBoardTest.GetTorqueWithPendulumJointRelativeToParentLink
6: [Msg] Loading SDF world file[/home/acroci/repos/gz-sim-yarp-plugins/tests/controlboard/pendulum_joint_relative_to_parent_link.sdf].
6: [Msg] Serving entity system service on [/entity/system/add]
6: [INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device  loaded from yarpConfigurationFile :  ../../../tests/controlboard/conf/gazebo_controlboard.ini 
6: [DEBUG] gz-sim-yarp-controlboard-system : robot scoped name:  model/single_pendulum
6: [DEBUG] gz-sim-yarp-controlboard-system : device scoped name:  model/single_pendulum/controlboard_plugin_device
6: [DEBUG] gz-sim-yarp-controlboard-system: initialConfiguration:  0.0 0.0
6: [DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (GAZEBO_PIDS (Pid0 1000.0 2.0 0.100000000000000005551 9999 9999 9 9)) (GAZEBO_VELOCITY_PIDS (Pid0 500.0 2.0 0.100000000000000005551 9999 9999 9 9)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (initialConfiguration "0.0 0.0") (jointNames upper_joint) (name "model/single_pendulum/controlboard_plugin_device") (robotScopedName "model/single_pendulum") (yarpDeviceName controlboard_plugin_device)
6: [INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
6: [INFO] Found 1 joints from the plugin configuration.
6: [INFO] Found 1 joints from the model description.
6: [INFO] Joint  upper_joint  added to the control board data.
6: [INFO] Registered YARP device with instance name: model/single_pendulum/controlboard_plugin_device
6: [Dbg] [SystemManager.cc:74] Loaded system [gzyarp::ControlBoard] for entity [8]
6: [Msg] Loaded level [3]
6: [Msg] No systems loaded from SDF, loading defaults
6: [Dbg] [ServerConfig.cc:1062] Loaded (3) plugins from file [/home/acroci/.gz/sim/7/server.config]
6: [Dbg] [Physics.cc:880] Loaded [gz::physics::dartsim::Plugin] from library [/home/acroci/mambaforge/envs/gz7/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so]
6: [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::Physics] for entity [1]
6: [Msg] Create service on [/world/turorial_controlboard/create]
6: [Msg] Remove service on [/world/turorial_controlboard/remove]
6: [Msg] Pose service on [/world/turorial_controlboard/set_pose]
6: [Msg] Pose service on [/world/turorial_controlboard/set_pose_vector]
6: [Msg] Light configuration service on [/world/turorial_controlboard/light_config]
6: [Msg] Physics service on [/world/turorial_controlboard/set_physics]
6: [Msg] SphericalCoordinates service on [/world/turorial_controlboard/set_spherical_coordinates]
6: [Msg] Enable collision service on [/world/turorial_controlboard/enable_collision]
6: [Msg] Disable collision service on [/world/turorial_controlboard/disable_collision]
6: [Msg] Material service on [/world/turorial_controlboard/visual_config]
6: [Msg] Material service on [/world/turorial_controlboard/wheel_slip]
6: [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::UserCommands] for entity [1]
6: [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::SceneBroadcaster] for entity [1]
6: [Msg] Serving world controls on [/world/turorial_controlboard/control], [/world/turorial_controlboard/control/state] and [/world/turorial_controlboard/playback/control]
6: [Msg] Serving GUI information on [/world/turorial_controlboard/gui/info]
6: [Msg] World [turorial_controlboard] initialized with [default_physics] physics profile.
6: [Msg] Serving world SDF generation service on [/world/turorial_controlboard/generate_world_sdf]
6: [Msg] Serving world names on [/gazebo/worlds]
6: [Msg] Resource path add service on [/gazebo/resource_paths/add].
6: [Msg] Resource path get service on [/gazebo/resource_paths/get].
6: [Msg] Resource path resolve service on [/gazebo/resource_paths/resolve].
6: [Msg] Resource paths published on [/gazebo/resource_paths].
6: [Msg] Server control service on [/server_control].
6: ========== configuring test
6: ========== test configured
6: [Msg] Found no publishers on /stats, adding root stats topic
6: [Msg] Found no publishers on /clock, adding root clock topic
6: [Dbg] [SimulationRunner.cc:503] Creating PostUpdate worker threads: 4
6: [Dbg] [SimulationRunner.cc:514] Creating postupdate worker thread (0)
6: [Dbg] [SimulationRunner.cc:514] Creating postupdate worker thread (1)
6: [Dbg] [SimulationRunner.cc:514] Creating postupdate worker thread (2)
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999851
6: ========== Test PostUpdate done
6: [Msg] Serving scene information on [/world/turorial_controlboard/scene/info]
6: [Msg] Serving graph information on [/world/turorial_controlboard/scene/graph]
6: [Msg] Serving full state on [/world/turorial_controlboard/state]
6: [Msg] Serving full state (async) on [/world/turorial_controlboard/state_async]
6: [Msg] Publishing scene information on [/world/turorial_controlboard/scene/info]
6: [Msg] Publishing entity deletions on [/world/turorial_controlboard/scene/deletion]
6: [Msg] Publishing state changes on [/world/turorial_controlboard/state]
6: [Msg] Publishing pose messages on [/world/turorial_controlboard/pose/info]
6: [Msg] Publishing dynamic pose messages on [/world/turorial_controlboard/dynamic_pose/info]
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999813
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999738
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999636
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999505
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999344
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0999154
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0998935
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0998686
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0998409
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0998102
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0997766
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.09974
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0997006
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0996582
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0996129
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0995647
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0995136
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0994596
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0994026
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0993428
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.09928
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0992143
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0991457
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0990742
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0989998
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0989224
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0988422
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.098759
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.098673
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.098584
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0984922
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0983974
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0982997
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0981992
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0980957
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0979894
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0978801
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.097768
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.097653
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.097535
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0974142
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0972905
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.097164
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0970345
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0969022
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0967669
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0966289
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.0964879
6: ========== Test PostUpdate done
6: ========== Test PreUpdate
6: ========== Test PreUpdate done
6: ========== Test PostUpdate
6: Torque measured: 0.1 - expected: 0.096344
6: ========== Test PostUpdate done
6: [Dbg] [SimulationRunner.cc:530] [Dbg] [SimulationRunner.cc:530] [Dbg] [SimulationRunner.cc:530] Exiting postupdate worker thread (0)
6: [Dbg] [SimulationRunner.cc:530] [Dbg] [SimulationRunner.cc:530] Exiting postupdate worker thread (1)
6: Exiting postupdate worker thread (2)
6: [       OK ] ControlBoardTest.GetTorqueWithPendulumJointRelativeToParentLink (2070 ms)
6: [----------] 1 test from ControlBoardTest (2070 ms total)
6: 
6: [----------] Global test environment tear-down
6: [==========] 1 test from 1 test suite ran. (2070 ms total)
6: [  PASSED  ] 1 test.
1/1 Test #6: ControlBoardTest .................   Passed    2.11 sec

The test for the second value is failing.

tests/controlboard/ControlBoardTest.cc

traversaro

Minor comments, for me it is ready to be merged.

xela-95 · 2024-02-20T14:05:19Z

I've seen that on macos the test on the controlboard is failing: https://github.com/robotology/gz-sim-yarp-plugins/actions/runs/7974385463/job/21770152674?pr=86#step:12:40257

It seems to be related to a lock issue... strange that is happening only there, maybe there is still a mutex error that happens in a transient way 🤔

traversaro · 2024-02-20T14:07:59Z

Based on https://stackoverflow.com/questions/66773247/libcabi-dylib-terminating-with-uncaught-exception-of-type-std-1system-er, perhaps we have some static mutex somewhere? If we can't find the culprit easily, I have also a macos mini you can used for debug. If we can't find the solution, we could also just suppress the test on macos for the time being.

xela-95 · 2024-02-20T14:43:47Z

Based on https://stackoverflow.com/questions/66773247/libcabi-dylib-terminating-with-uncaught-exception-of-type-std-1system-er, perhaps we have some static mutex somewhere? If we can't find the culprit easily, I have also a macos mini you can used for debug. If we can't find the solution, we could also just suppress the test on macos for the time being.

Yep we use static mutexes in every singleton class of each plugin, plus the robotinterface singleton. Do you suggest any tool useful to debug such errors?

To avoid error #86 (comment)

xela-95 · 2024-02-20T15:37:26Z

With commit efe775d I disabled the second unit test, in order to make the CI pass. This issue will be addressed in #90, since it is unrelated to the current issue and it could affect all plugins.

xela-95 self-assigned this Feb 14, 2024

xela-95 linked an issue Feb 14, 2024 that may be closed by this pull request

ControlBoard: fix torque feedback #81

Closed

xela-95 added 11 commits February 20, 2024 14:35

Get joint torque value from JointTransmittedWrench returned by gz-sim

0f1f77b

Add first version of unit test for ITorqueControl

64155f4

Update torque test and SDF model

c3d630b

WIP Improve unit test logs

4ad5484

Add fixed joint to base of the pendulum

f441b8e

Fix error sign error in expected torque equation

6043bef

Adapt SDF model to define joint frame relative to an arbitrary frame

5ce2d31

Log joint axis and transmitted wrench

b41cf1e

Refactor by using fixture class

9199fec

WIP Use a parametrized test

b84e162

The test for the second value is failing.

Remove control board data from singleton on ControlBoard destructor

7f99132

xela-95 force-pushed the feature/torque-feedback branch from 13f35ba to 7f99132 Compare February 20, 2024 13:37

xela-95 requested a review from traversaro February 20, 2024 13:41

xela-95 added the phase:implementation label Feb 20, 2024

traversaro reviewed Feb 20, 2024

View reviewed changes

tests/controlboard/ControlBoardTest.cc Show resolved Hide resolved

traversaro approved these changes Feb 20, 2024

View reviewed changes

xela-95 marked this pull request as ready for review February 20, 2024 13:53

Comment out second argument of unit test

efe775d

To avoid error #86 (comment)

xela-95 mentioned this pull request Feb 20, 2024

Controlboard unit test failing on MacOs #90

Open

xela-95 merged commit fe0b137 into main Feb 20, 2024
6 of 7 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Get joint torque value from TransmittedJointTorque returned by gz-sim #86

Get joint torque value from TransmittedJointTorque returned by gz-sim #86

xela-95 commented Feb 14, 2024

codecov bot commented Feb 14, 2024 •

edited

Loading

xela-95 commented Feb 16, 2024 •

edited

Loading

xela-95 commented Feb 16, 2024

traversaro left a comment

xela-95 commented Feb 20, 2024

traversaro commented Feb 20, 2024

xela-95 commented Feb 20, 2024

xela-95 commented Feb 20, 2024

Get joint torque value from TransmittedJointTorque returned by gz-sim #86

Get joint torque value from TransmittedJointTorque returned by gz-sim #86

Conversation

xela-95 commented Feb 14, 2024

codecov bot commented Feb 14, 2024 • edited Loading

Codecov Report

xela-95 commented Feb 16, 2024 • edited Loading

xela-95 commented Feb 16, 2024

traversaro left a comment

Choose a reason for hiding this comment

xela-95 commented Feb 20, 2024

traversaro commented Feb 20, 2024

xela-95 commented Feb 20, 2024

xela-95 commented Feb 20, 2024

codecov bot commented Feb 14, 2024 •

edited

Loading

xela-95 commented Feb 16, 2024 •

edited

Loading