Removes test-only arguments; adds more comments.

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

@@ -268,13 +268,7 @@ class RobotModel
    * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...)
    * will actually warp wrist (and all its descendants).
    */
-  static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link, Eigen::Isometry3d& transform,
-                                                             const JointModelGroup* jmg = nullptr);
-  static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link, const JointModelGroup* jmg = nullptr)
-  {
-    Eigen::Isometry3d unused;
-    return getRigidlyConnectedParentLinkModel(link, unused, jmg);
-  }
+  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,13 +1364,12 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
   return nullptr;
 }
 
-const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link, Eigen::Isometry3d& transform,
+const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link,
                                                                 const JointModelGroup* jmg)
 {
   if (!link)
     return link;
 
-  transform.setIdentity();
   const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
   const moveit::core::JointModel* joint = link->getParentJointModel();
   decltype(jmg->getJointModels().cbegin()) begin{}, end{};
@@ -1380,6 +1379,8 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel*
     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;
@@ -1391,7 +1392,6 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel*
 
   while (parent_link && is_fixed_or_not_in_jmg(joint))
   {
-    transform = link->getJointOriginTransform() * transform;
     link = parent_link;
     joint = link->getParentJointModel();
     parent_link = joint->getParentLinkModel();

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

@@ -1230,11 +1230,9 @@ class RobotState
    *
    * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel,
    * but can additionally resolve parents for attached objects / subframes.
-   *
-   * If transform is specified, return the (fixed) relative transform from the returned parent link to frame.
    */
   const moveit::core::LinkModel*
-  getRigidlyConnectedParentLinkModel(const std::string& frame, Eigen::Isometry3d* transform = nullptr,
+  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.
@@ -1716,6 +1714,14 @@ 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/robot_state.cpp

@@ -907,52 +907,42 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
   }
 }
 
-const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame, Eigen::Isometry3d* transform,
-                                                                const moveit::core::JointModelGroup* jmg) const
+const LinkModel*
+getLinkModelIncludingAttachedBodies(const std::string& frame) const
 {
-  const LinkModel* link{ nullptr };
-
+  // If the frame is a link, return that link.
   if (getRobotModel()->hasLinkModel(frame))
   {
-    link = getLinkModel(frame);
-    if (transform)
-      transform->setIdentity();
+    return getLinkModel(frame);
   }
-  else if (const auto it = attached_body_map_.find(frame); it != attached_body_map_.end())
+
+  // 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 };
-    link = body->getAttachedLink();
-    if (transform)
-      *transform = body->getPose();
+    return body->getAttachedLink();
   }
-  else
+
+  // 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_)
   {
-    bool found = false;
-    for (const auto& it : attached_body_map_)
+    const auto& body{ it.second };
+    if (body->hasSubframeTransform(frame))
     {
-      const auto& body{ it.second };
-      const Eigen::Isometry3d& subframe = body->getSubframeTransform(frame, &found);
-      if (found)
-      {
-        if (transform)  // prepend the body transform
-          *transform = body->getPose() * subframe;
-        link = body->getAttachedLink();
-        break;
-      }
+      return body->getAttachedLink();
     }
-    if (!found)
-      return nullptr;
   }
 
-  // link is valid and transform describes pose of frame w.r.t. global frame
-  Eigen::Isometry3d link_transform;
-  auto* parent = getRobotModel()->getRigidlyConnectedParentLinkModel(link, link_transform, jmg);
-  if (parent && transform)
-  {
-    // prepend link_transform to get transform from parent link to frame
-    *transform = link_transform * *transform;
-  }
-  return parent;
+  // 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)

moveit_core/robot_state/test/robot_state_test.cpp

@@ -755,9 +755,7 @@ TEST_F(OneRobot, rigidlyConnectedParent)
   state.setToDefaultValues();
 
   Eigen::Isometry3d a_to_b;
-  EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b", &a_to_b), link_a);
-  // translation from link_a to link_b is (0 0.5 0)
-  EXPECT_NEAR_TRACED(a_to_b.translation(), Eigen::Translation3d(0, 0.5, 0).translation());
+  EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);
 
   // attach "object" with "subframe" to link_b
   state.attachBody(std::make_unique<moveit::core::AttachedBody>(
@@ -768,9 +766,7 @@ TEST_F(OneRobot, rigidlyConnectedParent)
   // RobotState's version should resolve these too
   Eigen::Isometry3d transform;
   EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object"));
-  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe", &transform));
-  // transform from link_b to object/subframe is (1 0 1)
-  EXPECT_NEAR_TRACED((a_to_b.inverse() * transform).matrix(), Eigen::Isometry3d(Eigen::Translation3d(1, 0, 1)).matrix());
+  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe"));
 
   // test failure cases
   EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object"));