Reverts #2985, Ports moveit #3388 #3470 #3539 #3284

rr-mark · 2025-01-30T14:59:53Z

Description

Fixes RobotState::getRigidlyConnectedParentLinkModel and subframes: PRs not ported from moveit1. #3279
As Ports a6f8bb9 331381a 593a119 and 593a119 from moveit1 #3280, but with history correctly preserved. (Also includes a few commits in the same PRs, which Ports a6f8bb9 331381a 593a119 and 593a119 from moveit1 #3280 missed)
N.B. does not include changes to cartesian_path_service_capability, as that has changed significantly since the Moveit1 PRs were made.
Please back-port to Humble and Jazzy when merged

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

…2985)" This reverts commit 1f23344.

So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame. This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an active joint from the arm's tooltip. However, as long as this joint is not part of the JMG, the corresponding transform can be considered as fixed as well. This PR generalizes the functions getRigidlyConnectedParentLinkModel() in RobotState and RobotModel to receive an optional JMG pointer. If present, only (active) joints from that group are considered non-fixed. This PR also enables subframe support for setFromIK - simply by using getRigidlyConnectedParentLinkModel(), which already supported that. There is one drawback of this approach: A repeated application of setFromIK with the same target frame and JMG (as in computeCartesianPath()), will repeat the search for the common fixed parent link. Additionally, the passed RobotState needs to be up-to-date. We could mitigate this by pulling the corresponding code into a separate function and calling it once in computeCartesianPath().

…inkModel() Fixes #3388

find links with slashes again

JafarAbdi

Thanks!

codecov-commenter · 2025-01-30T15:10:30Z

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 88.37209% with 5 lines in your changes missing coverage. Please review.

Project coverage is 44.84%. Comparing base (fbdd8c5) to head (4ac24b0).
Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
moveit_core/robot_model/src/robot_model.cpp	75.00%	3 Missing ⚠️
...it_core/robot_state/src/cartesian_interpolator.cpp	0.00%	1 Missing ⚠️
moveit_core/robot_state/test/robot_state_test.cpp	94.12%	1 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3284      +/-   ##
==========================================
- Coverage   45.60%   44.84%   -0.76%     
==========================================
  Files         716      716              
  Lines       62388    62414      +26     
  Branches     7547     7555       +8     
==========================================
- Hits        28446    27983     -463     
- Misses      33776    34261     +485     
- Partials      166      170       +4

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

…ntTrajectory.

sjahr

Thanks for porting this over! I hate to ask but do you mind adding a bit documentation here? Moveit core is already complicated enough and these changes don't simplify it 😅

moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp

moveit_core/robot_model/src/robot_model.cpp

moveit_core/robot_state/src/robot_state.cpp

rr-mark · 2025-01-31T08:19:01Z

Thanks for porting this over! I hate to ask but do you mind adding a bit documentation here? Moveit core is already complicated enough and these changes don't simplify it 😅

Sure thing. None of this was originally my code, but I'll see what I can do later today.

sjahr · 2025-01-31T08:22:10Z

Sure thing. None of this was originally my code, but I'll see what I can do later today.

Yes, I am aware of this. Thanks a lot for doing it and let me know when you need help!

rr-mark · 2025-01-31T13:01:02Z

It looks like the dubious transform arguments are only there for unit tests. I'm trying to work out how to calculate them separately, so they can be deleted.

rr-mark · 2025-01-31T13:09:25Z

Actually, scratch that. Any alternate code to calculate the transform would only be used in the tests. I think I'm just going to delete these arguments.

sjahr

Thank you!

sjahr · 2025-02-05T16:07:01Z

@rr-mark Thank's a lot for doing all the work here! Last request: Do you mind squashing all commits authored by only together? I am going to merge this PR with a merge commit to preserver the commits ported over from moveit1 and I want to keep the history on main clean.

* Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985)" This reverts commit 1f23344. * Merge PR #3388: Generalize RobotState::setFromIK() So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame. This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an active joint from the arm's tooltip. However, as long as this joint is not part of the JMG, the corresponding transform can be considered as fixed as well. This PR generalizes the functions getRigidlyConnectedParentLinkModel() in RobotState and RobotModel to receive an optional JMG pointer. If present, only (active) joints from that group are considered non-fixed. This PR also enables subframe support for setFromIK - simply by using getRigidlyConnectedParentLinkModel(), which already supported that. There is one drawback of this approach: A repeated application of setFromIK with the same target frame and JMG (as in computeCartesianPath()), will repeat the search for the common fixed parent link. Additionally, the passed RobotState needs to be up-to-date. We could mitigate this by pulling the corresponding code into a separate function and calling it once in computeCartesianPath(). * Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentLinkModel() Fixes #3388 * Merge pull request #3539 from v4hn/find-links-with-slashes-again find links with slashes again * Ports to ROS2 and fixes problems introduced in merge conflicts. * Fixes formatting. * Makes robot_state_test.cpp include gmock. * Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::JointTrajectory. * Adds braces to make clang-tidy happy. * Removes test-only arguments; adds more comments. * Fixes formatting. * Fixes formatting. * Adds missing class scope. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> (cherry picked from commit 1794b8e) # Conflicts: # moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp # moveit_core/robot_state/CMakeLists.txt # moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp # moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp # moveit_core/robot_state/src/cartesian_interpolator.cpp # moveit_core/robot_state/src/robot_state.cpp # moveit_core/robot_state/test/robot_state_test.cpp

* Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985)" This reverts commit 1f23344. * Merge PR #3388: Generalize RobotState::setFromIK() So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame. This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an active joint from the arm's tooltip. However, as long as this joint is not part of the JMG, the corresponding transform can be considered as fixed as well. This PR generalizes the functions getRigidlyConnectedParentLinkModel() in RobotState and RobotModel to receive an optional JMG pointer. If present, only (active) joints from that group are considered non-fixed. This PR also enables subframe support for setFromIK - simply by using getRigidlyConnectedParentLinkModel(), which already supported that. There is one drawback of this approach: A repeated application of setFromIK with the same target frame and JMG (as in computeCartesianPath()), will repeat the search for the common fixed parent link. Additionally, the passed RobotState needs to be up-to-date. We could mitigate this by pulling the corresponding code into a separate function and calling it once in computeCartesianPath(). * Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentLinkModel() Fixes #3388 * Merge pull request #3539 from v4hn/find-links-with-slashes-again find links with slashes again * Ports to ROS2 and fixes problems introduced in merge conflicts. * Fixes formatting. * Makes robot_state_test.cpp include gmock. * Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::JointTrajectory. * Adds braces to make clang-tidy happy. * Removes test-only arguments; adds more comments. * Fixes formatting. * Fixes formatting. * Adds missing class scope. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> (cherry picked from commit 1794b8e)

* Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985)" This reverts commit 1f23344. * Merge PR #3388: Generalize RobotState::setFromIK() So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame. This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an active joint from the arm's tooltip. However, as long as this joint is not part of the JMG, the corresponding transform can be considered as fixed as well. This PR generalizes the functions getRigidlyConnectedParentLinkModel() in RobotState and RobotModel to receive an optional JMG pointer. If present, only (active) joints from that group are considered non-fixed. This PR also enables subframe support for setFromIK - simply by using getRigidlyConnectedParentLinkModel(), which already supported that. There is one drawback of this approach: A repeated application of setFromIK with the same target frame and JMG (as in computeCartesianPath()), will repeat the search for the common fixed parent link. Additionally, the passed RobotState needs to be up-to-date. We could mitigate this by pulling the corresponding code into a separate function and calling it once in computeCartesianPath(). * Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentLinkModel() Fixes #3388 * Merge pull request #3539 from v4hn/find-links-with-slashes-again find links with slashes again * Ports to ROS2 and fixes problems introduced in merge conflicts. * Fixes formatting. * Makes robot_state_test.cpp include gmock. * Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::JointTrajectory. * Adds braces to make clang-tidy happy. * Removes test-only arguments; adds more comments. * Fixes formatting. * Fixes formatting. * Adds missing class scope. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> (cherry picked from commit 1794b8e) Co-authored-by: Mark Johnson <[email protected]>

* Reverts #2985, Ports moveit #3388 #3470 #3539 (#3284) * Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985)" This reverts commit 1f23344. * Merge PR #3388: Generalize RobotState::setFromIK() So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame. This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an active joint from the arm's tooltip. However, as long as this joint is not part of the JMG, the corresponding transform can be considered as fixed as well. This PR generalizes the functions getRigidlyConnectedParentLinkModel() in RobotState and RobotModel to receive an optional JMG pointer. If present, only (active) joints from that group are considered non-fixed. This PR also enables subframe support for setFromIK - simply by using getRigidlyConnectedParentLinkModel(), which already supported that. There is one drawback of this approach: A repeated application of setFromIK with the same target frame and JMG (as in computeCartesianPath()), will repeat the search for the common fixed parent link. Additionally, the passed RobotState needs to be up-to-date. We could mitigate this by pulling the corresponding code into a separate function and calling it once in computeCartesianPath(). * Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentLinkModel() Fixes #3388 * Merge pull request #3539 from v4hn/find-links-with-slashes-again find links with slashes again * Ports to ROS2 and fixes problems introduced in merge conflicts. * Fixes formatting. * Makes robot_state_test.cpp include gmock. * Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::JointTrajectory. * Adds braces to make clang-tidy happy. * Removes test-only arguments; adds more comments. * Fixes formatting. * Fixes formatting. * Adds missing class scope. --------- Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Robert Haschke <[email protected]> Co-authored-by: Michael Görner <[email protected]> (cherry picked from commit 1794b8e) * Resolves merge conflicts. (#3323) --------- Co-authored-by: Mark Johnson <[email protected]>

rr-mark and others added 5 commits January 30, 2025 13:49

Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (moveit#…

439c619

…2985)" This reverts commit 1f23344.

Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentL…

b4d00ec

…inkModel() Fixes #3388

Merge pull request #3539 from v4hn/find-links-with-slashes-again

f6d5bff

find links with slashes again

Ports to ROS2 and fixes problems introduced in merge conflicts.

ce74986

JafarAbdi approved these changes Jan 30, 2025

View reviewed changes

Fixes formatting.

175a396

rr-mark mentioned this pull request Jan 30, 2025

Ports a6f8bb9 331381a 593a119 and 593a119 from moveit1 #3280

Closed

6 tasks

rr-mark added 2 commits January 30, 2025 15:17

Makes robot_state_test.cpp include gmock.

683b4af

Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::Joi…

d761ed5

…ntTrajectory.

sjahr reviewed Jan 30, 2025

View reviewed changes

Adds braces to make clang-tidy happy.

6544e40

rr-mark added 4 commits January 31, 2025 13:37

Removes test-only arguments; adds more comments.

4170979

Fixes formatting.

13c8467

Fixes formatting.

c8b7675

Adds missing class scope.

819c000

sea-bass requested a review from sjahr February 4, 2025 12:45

sea-bass added backport-humble Mergify label that triggers a PR backport to Humble backport-jazzy Mergify label that triggers a PR backport to Jazzy labels Feb 4, 2025

sjahr approved these changes Feb 5, 2025

View reviewed changes

Merge branch 'main' into ports_moveit_#3388_#3470_#3539

4ac24b0

sea-bass added this pull request to the merge queue Feb 6, 2025

Merged via the queue into moveit:main with commit 1794b8e Feb 6, 2025
8 of 9 checks passed

mergify bot mentioned this pull request Feb 6, 2025

Reverts #2985, Ports moveit #3388 #3470 #3539 (backport #3284) #3319

Merged

6 tasks

mergify bot mentioned this pull request Feb 6, 2025

Reverts #2985, Ports moveit #3388 #3470 #3539 (backport #3284) #3320

Merged

6 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Reverts #2985, Ports moveit #3388 #3470 #3539 #3284

Reverts #2985, Ports moveit #3388 #3470 #3539 #3284

rr-mark commented Jan 30, 2025 •

edited

Loading

JafarAbdi left a comment

codecov-commenter commented Jan 30, 2025 •

edited

Loading

sjahr left a comment

rr-mark commented Jan 31, 2025

sjahr commented Jan 31, 2025

rr-mark commented Jan 31, 2025

rr-mark commented Jan 31, 2025

sjahr left a comment

sjahr commented Feb 5, 2025

Reverts #2985, Ports moveit #3388 #3470 #3539 #3284

Reverts #2985, Ports moveit #3388 #3470 #3539 #3284

Conversation

rr-mark commented Jan 30, 2025 • edited Loading

Description

Checklist

JafarAbdi left a comment

Choose a reason for hiding this comment

codecov-commenter commented Jan 30, 2025 • edited Loading

Codecov Report

sjahr left a comment

Choose a reason for hiding this comment

rr-mark commented Jan 31, 2025

sjahr commented Jan 31, 2025

rr-mark commented Jan 31, 2025

rr-mark commented Jan 31, 2025

sjahr left a comment

Choose a reason for hiding this comment

sjahr commented Feb 5, 2025

rr-mark commented Jan 30, 2025 •

edited

Loading

codecov-commenter commented Jan 30, 2025 •

edited

Loading