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

* 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]>

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

@@ -255,6 +255,9 @@ class RobotModel
   LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr);
 
   /** \brief Get the latest link upwards the kinematic tree, which is only connected via fixed joints
+   *
+   * If jmg is given, all links that are not active in this JMG are considered fixed.
+   * Otherwise only fixed joints are considered fixed.
    *
    * This is useful, if the link should be warped to a specific pose using updateStateWithLinkAt().
    * As updateStateWithLinkAt() warps only the specified link and its descendants, you might not
@@ -265,7 +268,8 @@ class RobotModel
    * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...)
    * will actually warp wrist (and all its descendants).
    */
-  static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link);
+  static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
+                                                             const JointModelGroup* jmg = nullptr);
 
   /** \brief Get the array of links  */
   const std::vector<const LinkModel*>& getLinkModels() const

moveit_core/robot_model/src/robot_model.cpp

@@ -1364,14 +1364,32 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
   return nullptr;
 }
 
-const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link)
+const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link, const JointModelGroup* jmg)
 {
   if (!link)
     return link;
+
   const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
   const moveit::core::JointModel* joint = link->getParentJointModel();
+  decltype(jmg->getJointModels().cbegin()) begin{}, end{};
+  if (jmg)
+  {
+    begin = jmg->getJointModels().cbegin();
+    end = jmg->getJointModels().cend();
+  }
+
+  // Returns whether `joint` is part of the rigidly connected chain.
+  // This is only false if the joint is both in `jmg` and not fixed.
+  auto is_fixed_or_not_in_jmg = [begin, end](const JointModel* joint) {
+    if (joint->getType() == JointModel::FIXED)
+      return true;
+    if (begin != end &&                       // we do have a non-empty jmg
+        std::find(begin, end, joint) == end)  // joint does not belong to jmg
+      return true;
+    return false;
+  };
 
-  while (parent_link && joint->getType() == moveit::core::JointModel::FIXED)
+  while (parent_link && is_fixed_or_not_in_jmg(joint))
   {
     link = parent_link;
     joint = link->getParentJointModel();

moveit_core/robot_state/CMakeLists.txt

@@ -37,7 +37,7 @@ if(BUILD_TESTING)
         "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils")
   endif()
 
-  ament_add_gtest(test_robot_state test/robot_state_test.cpp
+  ament_add_gmock(test_robot_state test/robot_state_test.cpp
                   APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}")
 
   target_link_libraries(test_robot_state moveit_test_utils moveit_utils

moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp

@@ -167,13 +167,6 @@ class AttachedBody
    * The returned transform is guaranteed to be a valid isometry. */
   const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
 
-  /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link)
-   *
-   * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's
-   * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false).
-   * The returned transform is guaranteed to be a valid isometry. */
-  const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const;
-
   /** \brief Get the fixed transform to a named subframe on this body, relative to the world frame.
    * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's
    * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false).

moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp

@@ -1231,7 +1231,8 @@ class RobotState
    * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel,
    * but can additionally resolve parents for attached objects / subframes.
    */
-  const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const;
+  const moveit::core::LinkModel*
+  getRigidlyConnectedParentLinkModel(const std::string& frame, const moveit::core::JointModelGroup* jmg = nullptr) const;
 
   /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel.
    *   This is typically the root link of the URDF unless a virtual joint is present.
@@ -1712,6 +1713,13 @@ class RobotState
   /** \brief This function is only called in debug mode */
   bool checkCollisionTransforms() const;
 
+  /** \brief Get the closest link in the kinematic tree to `frame`.
+   *
+   * Helper function for getRigidlyConnectedParentLinkModel,
+   * which resolves attached objects / subframes.
+   */
+  const moveit::core::LinkModel* getLinkModelIncludingAttachedBodies(const std::string& frame) const;
+
   RobotModelConstPtr robot_model_;
 
   std::vector<double> position_;

moveit_core/robot_state/src/cartesian_interpolator.cpp

@@ -455,6 +455,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
     if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options,
                                cost_function))
     {
+      start_state->update();
       path.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
     }
     else

moveit_core/robot_state/src/robot_state.cpp

@@ -907,14 +907,41 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
   }
 }
 
-const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
+const LinkModel* RobotState::getLinkModelIncludingAttachedBodies(const std::string& frame) const
 {
-  bool found;
-  const LinkModel* link{ nullptr };
-  getFrameInfo(frame, link, found);
-  if (!found)
-    RCLCPP_ERROR(getLogger(), "Unable to find link for frame '%s'", frame.c_str());
-  return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
+  // If the frame is a link, return that link.
+  if (getRobotModel()->hasLinkModel(frame))
+  {
+    return getLinkModel(frame);
+  }
+
+  // If the frame is an attached body, return the link the body is attached to.
+  if (const auto it = attached_body_map_.find(frame); it != attached_body_map_.end())
+  {
+    const auto& body{ it->second };
+    return body->getAttachedLink();
+  }
+
+  // If the frame is a subframe of an attached body, return the link the body is attached to.
+  for (const auto& it : attached_body_map_)
+  {
+    const auto& body{ it.second };
+    if (body->hasSubframeTransform(frame))
+    {
+      return body->getAttachedLink();
+    }
+  }
+
+  // If the frame is none of the above, return nullptr.
+  return nullptr;
+}
+
+const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame,
+                                                                const moveit::core::JointModelGroup* jmg) const
+{
+  const LinkModel* link = getLinkModelIncludingAttachedBodies(frame);
+
+  return getRobotModel()->getRigidlyConnectedParentLinkModel(link, jmg);
 }
 
 const Eigen::Isometry3d& RobotState::getJointTransform(const JointModel* joint)