Added equality constraint services to callbacks

[acodeodyssey]

Added services to get/set equality constraint parameters

[DavidPL1]

The added tests seem to be failing:

Test output

```bash [ROSTEST]----------------------------------------------------------------------- [mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]- /media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:755 Expected equality of these values: m->eq_data[joint_eq_id * 11] Which is: 0.9 0.5 --------------------------------------------------------------------------------

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:756
Expected equality of these values:
m->eq_data[joint_eq_id * 11 + 1]
Which is: 0.95
0.25

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:757
Expected equality of these values:
m->eq_data[joint_eq_id * 11 +2]
Which is: 0.001
0.76

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:758
Expected equality of these values:
m->eq_data[joint_eq_id * 11 +3]
Which is: 0.5
0.66

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:759
Expected equality of these values:
m->eq_data[joint_eq_id * 11 +4]
Which is: 2
1

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0.9
data[i]
Which is: 1.1

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0.95
data[i]
Which is: 1.2

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0.001
data[i]
Which is: 1.3

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraint
Values][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0.5
data[i]
Which is: 2

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 2
data[i]
Which is: 3

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0
data[i]
Which is: 4

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0
data[i]
Which is: 1

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0
data[i]
Which is: 0.1

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0
data[i]
Which is: 0.2

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0
data[i]
Which is: 0.3

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:765
Expected equality of these values:
m->eq_data[weld_eq_id * 11+i]
Which is: 0
data[i]
Which is: 0.9

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:768
Expected equality of these values:
m->eq_solimp[weld_eq_id+j]
Which is: 0
solimp[j]
Which is: 0.8

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:768
Expected equality of these values:
m->eq_solimp[weld_eq_id+j]
Which is: 0.8
solimp[j]
Which is: 0.95

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:768
Expected equality of these values:
m->eq_solimp[weld_eq_id+j]
Which is: 0.95
solimp[j]
Which is: 0.002

---

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:768
Expected equality of these values:
m->eq_solimp[weld_eq_id+j]
Which is: 0.002
solimp[j]
Which is: 0.4

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:768
Expected equality of these values:
m->eq_solimp[weld_eq_id+j]
Which is: 0.4
solimp[j]
Which is: 2

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:770
Expected equality of these values:
m->eq_solref[weld_eq_id]
Which is: 0
0.3

[mujoco_ros.rosunit-ros_interface_test/InitialEualityConstraintValues][FAILURE]-
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:771
Expected equality of these values:
m->eq_solref[weld_eq_id+1]
Which is: 0.3
0.9

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:811
Expected equality of these values:
m->eq_solref[weld_eq_id]
Which is: 0
srv.request.parameters.solverParameters.timeconst
Which is: 0.2

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:812
Expected equality of these values:
m->eq_solref[weld_eq_id+1]
Which is: 0.2
srv.request.parameters.solverParameters.dampratio
Which is: 0.8

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:813
Expected equality of these values:
m->eq_solimp[weld_eq_id]
Which is: 0
srv.request.parameters.solverParameters.dampratio
Which is: 0.8

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:817
Expected equality of these values:
m->eq_solimp[weld_eq_id+3]
Which is: 0.001
srv.request.parameters.solverParameters.midpoint
Which is: 0.5

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:818
Expected equality of these values:
m->eq_solimp[weld_eq_id+4]
Which is: 0.5
srv.request.parameters.solverParameters.power
Which is: 2

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:819
Expected equality of these values:
m->eq_data[weld_eq_id*11]
Which is: 0.9
srv.request.parameters.relpose.position.x
Which is: 1.2

[mujoco_ros.rosunit-ros_interface_test/
SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:820
Expected equality of these values:
m->eq_data[weld_eq_id*11+1]
Which is: 0.95
srv.request.parameters.relpose.position.y
Which is: 1.3

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:821
Expected equality of these values:
m->eq_data[weld_eq_id*11+2]
Which is: 0.001
srv.request.parameters.relpose.position.z
Which is: 1.4

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:822
Expected equality of these values:
m->eq_data[weld_eq_id*11+3]
Which is: 0.5
srv.request.parameters.anchor[0]
Which is: 5

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:823
Expected equality of these values:
m->eq_data[weld_eq_id*11+4]
Which is: 2
srv.request.parameters.anchor[1]
Which is: 3

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:824
Expected equality of these values:
m->eq_data[weld_eq_id*11+5]
Which is: 0
srv.request.parameters.anchor[2]
Which is: 7

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:825
Expected equality of these values:
m->eq_data[weld_eq_id*11+6]
Which is: 0
srv.request.parameters.relpose.orientation.w
Which is: 1

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:826
Expected equality of these values:
m->eq_data[weld_eq_id*11+7]
Which is: 0
srv.request.parameters.relpose.orientation.x
Which is: 0.2

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:827
Expected equality of these values:
m->eq_data[weld_eq_id*11+8]
Which is: 0
srv.request.parameters.relpose.orientation.y
Which is: 0.3

[mujoco_ros.rosunit-ros_interface_test/SetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:828
Expected equality of these values:
m->eq_data[weld_eq_id*11+9]
Which is: 0
srv.request.parameters.relpose.orientation.z
Which is: 0.4

[mujoco_ros.rosunit-ros_interface_test/GetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:871
Expected equality of these values:
g_srv.response.parameters.anchor
Which is: {{ 1.1, 1.2, 1.3, 0.936586 }}
srv.request.parameters.anchor
Which is: {{ 2, 3, 4 }}

---

[mujoco_ros.rosunit-ros_interface_test/GetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:872
Expected equality of these values:
g_srv.response.parameters.element1
Which is: "world"
srv.request.parameters.element1
Which is: ""

[mujoco_ros.rosunit-ros_interface_test/GetEualityConstraintCallback][FAILURE]---
/media/fast/code/sim4dex/mujoco_ws/src/mujoco_ros_pkgs/mujoco_ros/test/ros_interface_test.cpp:874
Expected equality of these values:
g_srv.response.parameters.relpose
Which is: position:
x: 2
y: 3
z: 4
orientation:
x: 0.0936586
y: 0.187317
z: 0.280976
w: 0.936586

srv.request.parameters.relpose
Which is: position:
x: 1.1
y: 1.2
z: 1.3
orientation:
x: 0.1
y: 0.2
z: 0.3
w: 1

SUMMARY

• RESULT: FAIL
• TESTS: 17
• ERRORS: 0
• FAILURES: 3

test_results/mujoco_ros/rosunit-ros_interface_test.xml: 34 tests, 0 errors, 6 failures, 0 skipped

</details>

[DavidPL1]

I think it also makes more sense to have a list of constraint parameters for the get and set service calls to allow changing multiple constraints with the same call. We would then iterate over the list and call the current Getter and Setter callbacks for each item.

[DavidPL1]

We need to apply the formatting and update the plugin API to the newer version. Then I'll check the tests.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

❗ No coverage uploaded for pull request base (mocap-plugin-msgs@9265f4e). Click here to learn what that means.

Additional details and impacted files

@@                 Coverage Diff                  @@
##             mocap-plugin-msgs      #24   +/-   ##
====================================================
  Coverage                     ?   72.18%           
====================================================
  Files                        ?       10           
  Lines                        ?     1599           
  Branches                     ?        0           
====================================================
  Hits                         ?     1154           
  Misses                       ?      445           
  Partials                     ?        0           

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

[DavidPL1]

noetic ci seems to have a linker problem, but this only happens in GHA and will be fixed separately.
Format complains because of outdated focal clang-format and we are now targeting clang-format-14 (Jammy). This will be updated separately.

Thus, even though 2 checks fail, this is good to go. Merging.