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 tool tip. However, as long as this joint is not part of the
JMG, the corresponding transform can be considered as fixed.

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 sub frame support for setFromIK - simply by using
getRigidlyConnectedParentLinkModel(), which already supported that.

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -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 moveit::core::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

@@ -1176,14 +1176,29 @@ 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();
+  }
+
+  auto is_fixed_or_not_in_jmg = [begin, end](const moveit::core::JointModel* joint) {
+    if (joint->getType() == moveit::core::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/include/moveit/robot_state/robot_state.h

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

moveit_core/robot_state/src/cartesian_interpolator.cpp

@@ -160,7 +160,10 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
     // Explicitly use a single IK attempt only: We want a smooth trajectory.
     // Random seeding (of additional attempts) would probably create IK jumps.
     if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options))
+    {
+      start_state->update();
       traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
+    }
     else
       break;
 

moveit_core/robot_state/src/robot_state.cpp

@@ -797,7 +797,8 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
         global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
 }
 
-const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
+const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame, Eigen::Isometry3d* transform,
+                                                                const moveit::core::JointModelGroup* jmg) const
 {
   const moveit::core::LinkModel* link{ nullptr };
 
@@ -810,16 +811,35 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin
     auto body{ getAttachedBody(object) };
     if (!body->hasSubframeTransform(frame))
       return nullptr;
+    if (transform)
+      *transform = body->getGlobalSubframeTransform(frame);
     link = body->getAttachedLink();
   }
   else if (hasAttachedBody(frame))
   {
-    link = getAttachedBody(frame)->getAttachedLink();
+    auto body{ getAttachedBody(frame) };
+    if (transform)
+      *transform = body->getGlobalPose();
+    link = body->getAttachedLink();
   }
   else if (getRobotModel()->hasLinkModel(frame))
+  {
     link = getLinkModel(frame);
-
-  return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
+    if (transform)
+    {
+      BOOST_VERIFY(checkLinkTransforms());
+      *transform = global_link_transforms_[link->getLinkIndex()];
+    }
+  }
+  // link is valid and transform describes pose of frame w.r.t. global frame
+  auto* parent = getRobotModel()->getRigidlyConnectedParentLinkModel(link, jmg);
+  if (parent && transform)
+  {
+    BOOST_VERIFY(checkLinkTransforms());
+    // compute transform from parent link to frame
+    *transform = global_link_transforms_[parent->getLinkIndex()].inverse() * *transform;
+  }
+  return parent;
 }
 
 bool RobotState::satisfiesBounds(double margin) const
@@ -1657,8 +1677,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
     if (!setToIKSolverFrame(pose, solver))
       return false;
 
-    // try all of the solver's possible tip frames to see if they uniquely align with any of our passed in pose tip
-    // frames
+    // try all of the solver's possible tip frames to see if they match with any of the passed-in pose tip frames
     bool found_valid_frame = false;
     std::size_t solver_tip_id;  // our current index
     for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
@@ -1677,39 +1696,33 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
 
       if (pose_frame != solver_tip_frame)
       {
-        if (hasAttachedBody(pose_frame))
+        Eigen::Isometry3d pose_parent_to_frame;
+        auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame, &pose_parent_to_frame, jmg);
+        if (!pose_parent)
         {
-          const AttachedBody* body = getAttachedBody(pose_frame);
-          pose_frame = body->getAttachedLinkName();
-          pose = pose * body->getPose().inverse();
+          ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist.");
+          return false;
         }
-        if (pose_frame != solver_tip_frame)
+        Eigen::Isometry3d tip_parent_to_tip;
+        auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame, &tip_parent_to_tip, jmg);
+        if (!tip_parent)
         {
-          const moveit::core::LinkModel* link_model = getLinkModel(pose_frame);
-          if (!link_model)
-          {
-            ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist.");
-            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;
-            }
+          ROS_ERROR_STREAM_NAMED(LOGNAME, "Solver tip frame '" << solver_tip_frame << "' does not exist.");
+          return false;
         }
-
-      }  // end if pose_frame
-
-      // Check if this pose frame works
-      if (pose_frame == solver_tip_frame)
+        if (pose_parent == tip_parent)
+        {
+          // transform goal pose as target for solver_tip_frame (instead of pose_frame)
+          pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip;
+          found_valid_frame = true;
+          break;
+        }
+      }
+      else
       {
         found_valid_frame = true;
         break;
       }
-
     }  // end for solver_tip_frames
 
     // Make sure one of the tip frames worked

moveit_core/robot_state/test/robot_state_test.cpp

@@ -686,18 +686,25 @@ TEST_F(OneRobot, rigidlyConnectedParent)
   EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
 
   moveit::core::RobotState state(robot_model_);
+  state.update();
 
-  EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);
+  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());
 
   // attach "object" with "subframe" to link_b
   state.attachBody(std::make_unique<moveit::core::AttachedBody>(
-      link_b, "object", Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{},
+      link_b, "object", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)), std::vector<shapes::ShapeConstPtr>{},
       EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::JointTrajectory{},
-      moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } }));
+      moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } }));
 
   // 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"));
+  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());
 
   // test failure cases
   EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object"));