Fixes merge conflicts (humble backport 3077) (#3087)

Co-authored-by: Tom Noble <[email protected]>
moveit · Nov 15, 2024 · f07f427 · f07f427
1 parent 38299f6
commit f07f427
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 23 deletions.
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -1671,32 +1671,15 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
         auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame);
         if (!pose_parent)
         {
-          RCLCPP_ERROR_STREAM(getLogger(), "The following Pose Frame does not exist: " << pose_frame);
+          RCLCPP_ERROR_STREAM(LOGGER, "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)
         {
-<<<<<<< HEAD
-          const moveit::core::LinkModel* link_model = getLinkModel(pose_frame);
-          if (!link_model)
-          {
-            RCLCPP_ERROR(LOGGER, "The following Pose Frame does not exist: %s", pose_frame.c_str());
-            return false;
-          }
-          const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms();
-          for (const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
-            if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame))
-            {
-              pose_frame = solver_tip_frame;
-              pose = pose * fixed_link.second;
-              break;
-            }
-=======
-          RCLCPP_ERROR_STREAM(getLogger(), "The following Solver Tip Frame does not exist: " << solver_tip_frame);
+          RCLCPP_ERROR_STREAM(LOGGER, "The following Solver Tip Frame does not exist: " << solver_tip_frame);
           return false;
->>>>>>> ab34495d2 (Allow RobotState::setFromIK to work with subframes (#3077))
         }
         Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame);
         if (pose_parent == tip_parent)

diff --git a/...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
@@ -406,9 +406,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityColl
   const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_);
   Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_);
 
-  // Attach an object with no subframes, and no transform
+  // Attach an object with an ignored subframe, and no transform
   Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity();
-  attachToLink(state, tip_link, "object", object_pose_in_tip, { {} });
+  moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } });
+  attachToLink(state, tip_link, "object", object_pose_in_tip, subframes);
 
   // The RobotState should be able to use an object pose to set the joints
   Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip;
@@ -435,8 +436,8 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedC
   Eigen::Isometry3d object_pose_in_tip;
   object_pose_in_tip = Eigen::Translation3d(1, 2, 3);
   object_pose_in_tip *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitX());
-
-  attachToLink(state, tip_link, "object", object_pose_in_tip, { {} });
+  moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } });
+  attachToLink(state, tip_link, "object", object_pose_in_tip, subframes);
 
   // The RobotState should be able to use an object pose to set the joints
   Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip;