From 03953828256000622579b5ae771bd4fa30bc3660 Mon Sep 17 00:00:00 2001 From: Mark Johnson Date: Tue, 8 Jul 2025 15:27:16 +0100 Subject: [PATCH 1/2] Adds overload to getFrameTransform which takes destination and source frames; uses this in setFromIK. --- .../moveit/robot_state/robot_state.hpp | 20 +++++++++++ moveit_core/robot_state/src/robot_state.cpp | 35 ++++++++++++++++--- 2 files changed, 51 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp index 26ea89e8e2..14d622b0d3 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp @@ -1584,6 +1584,26 @@ class RobotState * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; + /** \brief Get the transformation matrix from the frame identified by \e source_frame_id to the frame identified by \e + * destination_frame_id + * + * If source_frame_id or destination_frame_id was not found, \e frame_found is set to false and an identity transform + * is returned. + * + * The returned transformation is always a valid isometry. */ + Eigen::Isometry3d getFrameTransform(const std::string& destination_frame_id, const std::string& source_frame_id, + bool* frames_found = nullptr); + + /** \brief Get the transformation matrix from the frame identified by \e source_frame_id to the frame identified by \e + * destination_frame_id + * + * If source_frame_id or destination_frame_id was not found, \e frame_found is set to false and an identity transform + * is returned. + * + * The returned transformation is always a valid isometry. */ + Eigen::Isometry3d getFrameTransform(const std::string& destination_frame_id, const std::string& source_frame_id, + bool* frames_found = nullptr) const; + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index ddb3fa830c..3e1a7461ad 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1335,6 +1335,34 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_ return result; } +Eigen::Isometry3d RobotState::getFrameTransform(const std::string& destination_frame_id, + const std::string& source_frame_id, bool* frames_found) +{ + updateLinkTransforms(); + return static_cast(this)->getFrameTransform(destination_frame_id, source_frame_id, frames_found); +} + +Eigen::Isometry3d RobotState::getFrameTransform(const std::string& destination_frame_id, + const std::string& source_frame_id, bool* frames_found) const +{ + bool found; + const Eigen::Isometry3d& destination_transform = getFrameTransform(destination_frame_id, &found); + if (!found) + { + if (frames_found) + *frames_found = false; + return destination_transform; // Will be the identity. + } + const Eigen::Isometry3d& source_transform = getFrameTransform(source_frame_id, &found); + if (!found) + { + if (frames_found) + *frames_found = false; + return source_transform; // Will be the identity. + } + return destination_transform.inverse() * source_transform; +} + const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, bool& frame_found) const { @@ -1927,18 +1955,17 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is RCLCPP_ERROR_STREAM(getLogger(), "The following Pose Frame does not exist: " << pose_frame); return false; } - Eigen::Isometry3d pose_parent_to_frame = getFrameTransform(pose_frame); auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame); if (!tip_parent) { RCLCPP_ERROR_STREAM(getLogger(), "The following Solver Tip Frame does not exist: " << solver_tip_frame); return false; } - Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame); if (pose_parent == tip_parent) { // transform goal pose as target for solver_tip_frame (instead of pose_frame) - pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip; + Eigen::Isometry3d pose_frame_from_solver_tip_frame = getFrameTransform(pose_frame, solver_tip_frame); + pose = pose * pose_frame_from_solver_tip_frame; found_valid_frame = true; break; } @@ -1948,7 +1975,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is found_valid_frame = true; break; } // end if pose_frame - } // end for solver_tip_frames + } // end for solver_tip_frames // Make sure one of the tip frames worked if (!found_valid_frame) From 1958998625051e165f2374a3b9e88fff6bc64a8a Mon Sep 17 00:00:00 2001 From: Mark Johnson Date: Tue, 8 Jul 2025 15:34:44 +0100 Subject: [PATCH 2/2] Undoes autoformat errors. --- moveit_core/robot_state/src/robot_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 3e1a7461ad..d91e150162 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1975,7 +1975,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is found_valid_frame = true; break; } // end if pose_frame - } // end for solver_tip_frames + } // end for solver_tip_frames // Make sure one of the tip frames worked if (!found_valid_frame)