Reverts #2985, Ports moveit #3388 #3470 #3539

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)