diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 6f406a8ca3..78614304a0 100644 --- 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& 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/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 52707ace2c..3a389fae1a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/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;