Fix RobotState::getRigidlyConnectedParentLinkModel() #2985
Merged
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Simplify function using getFrameInfo() to yield the link corresponding to the given frame. The order of frame resolution was wrong here: If a link frame containing a slash was given, the code was expecting an attached body with a subframe. As these didn't exist, the function falsely returned NULL.
rhaschke
added
the
backport-humble
|
Answering for @sjahr, this really looks like the issue we have been experiencing. This is a really good code simplification as well. Thank you! |
henningkayser
approved these changes
Sep 4, 2024
|
Is this automatically backported to Humble or should I do that manually? |
|
The backport should have been triggered.... Not sure what went wrong here. But we can also call mergify manually. Let me check |
|
@Mergifyio backport humble |
henningkayser
added
backport-humble
mergify bot
pushed a commit
that referenced
this pull request
Sep 5, 2024
Simplify function using getFrameInfo() to yield the link corresponding to the given frame. The order of frame resolution was wrong here: If a link frame containing a slash was given, the code was expecting an attached body with a subframe. As these didn't exist, the function falsely returned NULL. (cherry picked from commit 1f23344) # Conflicts: # moveit_core/robot_state/src/robot_state.cpp
rhaschke
pushed a commit
that referenced
this pull request
Sep 9, 2024
Simplify function using getFrameInfo() to yield the link corresponding to the given frame. The order of frame resolution was wrong here: If a link frame containing a slash was given, the code was expecting an attached body with a subframe. As these didn't exist, the function falsely returned NULL.
github-merge-queue bot
pushed a commit
that referenced
this pull request
Feb 6, 2025
* 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]>
mergify bot
pushed a commit
that referenced
this pull request
Feb 6, 2025
* 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
mergify bot
pushed a commit
that referenced
this pull request
Feb 6, 2025
* 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)
sea-bass
pushed a commit
that referenced
this pull request
Feb 6, 2025
* 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]>
sea-bass
pushed a commit
that referenced
this pull request
Feb 6, 2025
* 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]>
sussybot5258
pushed a commit
to GreyCatAI/moveit2
that referenced
this pull request
Apr 19, 2025
…it#3284) * Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (moveit#2985)" This reverts commit 1f23344. * Merge PR moveit#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 moveit#3470: Avoid global transforms in getRigidlyConnectedParentLinkModel() Fixes moveit#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]>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Simplify function using
getFrameInfo()to yield the link corresponding to the given frame.The order of frame resolution was wrong here:
If a link frame containing a slash was given, the code was expecting an attached body with a subframe. As these didn't exist, the function falsely returned
NULL.@sjahr: Maybe this is the error you recently observed? Note, that the function was already fixed in MoveIt1.