Enable using a subgroup of the move group in servo #2396
Conversation
There was a problem hiding this comment.
Thanks for getting on this! I think your idea of having a fixed move group and only allowing the changing of subgroups has reduced scope enough to be useful and also not overly complicated.
I have one meaningful inline comment about searching for indices (see below).
My other main comment is, I know @AndyZe, @ibrahiminfinite, and I were discussing the risks of immediately switching planning groups and having joints added/removed be commanded with sudden velocity changes. I think because of the subgroup assumption, this may be OK, but it's worth thinking about it: if, e.g., a joint was moving with a nonzero velocity and we suddenly change to a subgroup without the joint, will this cause any issue or will it slow down the joint using the scalings provided?
EDIT: Adding to the above, our "scoped down and not complicated" idea here was to enforce that servo was paused before allowing switching groups. So that's always a fallback.
Codecov ReportAttention:
Additional details and impacted files@@ Coverage Diff @@
## main #2396 +/- ##
==========================================
+ Coverage 50.84% 50.87% +0.03%
==========================================
Files 386 386
Lines 31938 31983 +45
==========================================
+ Hits 16237 16269 +32
- Misses 15701 15714 +13
☔ View full report in Codecov by Sentry. |
tylerjw
force-pushed
the
pr-enable_using_subgroup_in_servo
branch
from
a899bed to
3690cb7
Compare
| } | ||
|
|
||
| return params_valid; | ||
| return true; | ||
| } | ||
|
|
||
| bool Servo::updateParams() |
There was a problem hiding this comment.
Need to make sure Servo is paused before doing this for a couple reasons:
- Don't want to have these operations in the control loop
- I think it's good from a safety perspective to stop the other joints beside this subgroup. Pausing should do that
|
Does this change the dimension of the final command to ros2_control? That would be another issue when the controller topic type is |
From what I understand, the new logic does not change the dimension of the final command, it just sets the |
|
@AndyZe my changes are meant to enable servoing only a subgroup at runtime without the need to pause servo and/or switch controllers. If a subgroup is enabled I only use this subgroup to calculate the delta_thetas and command a 0 delta theta to the other joints of the move_group. From a robot perspective nothing should change except that some joints (the ones that aren't part of the subgroup) get commands to hold their current position. |
|
@ibrahiminfinite Any clue why the tests are failing? |
@ibrahiminfinite FYI: The CI failure was caused by a bug in an upstream dependency (ros-controls/ros2_control#1127) and it already got fixed on their end with a new minor release ros-controls/ros2_control@3.19.0...3.19.1, so our CI will be working again once the docker images got updated (Sebastian C just triggered that 🙏) |
Co-authored-by: Sebastian Castro <[email protected]>
|
It does seem there is another failure now, though... 😔 |
|
Now this unittest is failing which is not related to these changes (it fails in #2288 and main too). [moveit_servo_utils_test-5] [INFO] [1696613355.885158439] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[moveit_servo_utils_test-5] /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_ros/moveit_servo/tests/test_utils.cpp:162: Failure
[moveit_servo_utils_test-5] Expected equality of these values:
[moveit_servo_utils_test-5] scaling_result.second
[moveit_servo_utils_test-5] Which is: 1-byte object <02>
[moveit_servo_utils_test-5] moveit_servo::StatusCode::NO_WARNING
[moveit_servo_utils_test-5] Which is: 1-byte object <00>@ibrahiminfinite Do you have any idea why this is happening? I don't see any immediate reason for the flakiness but I am probably missing something |
That is weird! |
|
That was my impression too, thanks for your quick answer! Also when I run the test locally, it succeeds 🤔 |
Maybe the change in condition number is right at the threshold of the servo parameters? I would print the values in the condition numbers to see how small their differences are. You could consider maybe picking a bigger cartesian delta and/or thresholds in the servo params, or choosing an initial state with higher manipulability. It's OK to use bigger than default numbers here! |
|
I think I know what's wrong with the unit tests. (@ibrahiminfinite) I ran these locally, printing output, and have been noticing their values are not consistent between runs. Turns out, when you do This only sets the joint values but not the link transforms used in When I made this change, my results are consistent, but also leads to new test failures because for many of the tests the condition number actually evaluates to Up to you guys if you want to look at this for this PR, or in a follow on. |
|
I think I may have fixed it in #2414? |
* Enable using a subgroup of the move group in servo * Remove unnecessary validations since the param is const * Apply suggestions from code review Co-authored-by: Sebastian Castro <[email protected]> * Don't copy joints if subgroup == move group * Re-add params_valid in validateParams * Generalize active subgroup delta calculation * Add more efficient move group joint position lookup * Create subgroup map in the constructor * Apply suggestions from code review Co-authored-by: Sebastian Castro <[email protected]> * Update moveit_ros/moveit_servo/src/servo.cpp --------- Co-authored-by: Sebastian Castro <[email protected]>
Description
This PR enables using a subgroup of the move_group at runtime without the need to stop servo or switch controllers.
Checklist