Allow RobotState::setFromIK to work with subframes (backport #3077)

moveit_core/robot_state/src/robot_state.cpp

@@ -1761,14 +1761,17 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
 
       if (pose_frame != solver_tip_frame)
       {
-        if (hasAttachedBody(pose_frame))
+        auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame);
+        if (!pose_parent)
         {
-          const AttachedBody* body = getAttachedBody(pose_frame);
-          pose_frame = body->getAttachedLinkName();
-          pose = pose * body->getPose().inverse();
+          RCLCPP_ERROR_STREAM(getLogger(), "The following Pose Frame does not exist: " << pose_frame);
+          return false;
         }
-        if (pose_frame != solver_tip_frame)
+        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)
           {
@@ -1785,18 +1788,26 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
               break;
             }
           }
+=======
+          RCLCPP_ERROR_STREAM(getLogger(), "The following Solver Tip Frame does not exist: " << solver_tip_frame);
+          return false;
+>>>>>>> ab34495d2 (Allow RobotState::setFromIK to work with subframes (#3077))
         }
-
-      }  // end if pose_frame
-
-      // Check if this pose frame works
-      if (pose_frame == solver_tip_frame)
+        Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(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
+      }  // end if pose_frame
+    }    // end for solver_tip_frames
 
     // Make sure one of the tip frames worked
     if (!found_valid_frame)

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp

@@ -134,6 +134,35 @@ class TrajectoryFunctionsTestBase : public testing::Test
    */
   bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon);
 
+  /**
+   * @brief check if two sets of joint positions are close
+   * @param joints1 the first set of joint positions to compare
+   * @param joints2 the second set of joint positions to compare
+   * @param epsilon the tolerance a all joint position diffs must satisfy
+   * @return false if any joint diff exceeds tolerance. true otherwise
+   */
+  bool jointsNear(const std::vector<double>& joints1, const std::vector<double>& joints2, double epsilon);
+
+  /**
+   * @brief get the current joint values of the robot state
+   * @param jmg the joint model group whose joints we are interested in
+   * @param state the robot state to fetch the current joint positions for
+   * @return the joint positions for joints from jmg, set to the positions determined from state
+   */
+  std::vector<double> getJoints(const moveit::core::JointModelGroup* jmg, const moveit::core::RobotState& state);
+
+  /**
+   * @brief attach a collision object and subframes to a link
+   * @param state the state we are updating
+   * @param link the link we are attaching the collision object to
+   * @param object_name a unique name for the collision object
+   * @param object_pose the pose of the object relative to the parent link
+   * @param subframes subframe names and poses relative to the object they attach to
+   */
+  void attachToLink(moveit::core::RobotState& state, const moveit::core::LinkModel* link,
+                    const std::string& object_name, const Eigen::Isometry3d& object_pose,
+                    const moveit::core::FixedTransformsMap& subframes);
+
 protected:
   // ros stuff
   rclcpp::Node::SharedPtr node_;
@@ -166,6 +195,43 @@ bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const E
   return true;
 }
 
+bool TrajectoryFunctionsTestBase::jointsNear(const std::vector<double>& joints1, const std::vector<double>& joints2,
+                                             double epsilon)
+{
+  if (joints1.size() != joints2.size())
+  {
+    return false;
+  }
+  for (std::size_t i = 0; i < joints1.size(); ++i)
+  {
+    if (fabs(joints1.at(i) - joints2.at(i)) > fabs(epsilon))
+    {
+      return false;
+    }
+  }
+  return true;
+}
+
+std::vector<double> TrajectoryFunctionsTestBase::getJoints(const moveit::core::JointModelGroup* jmg,
+                                                           const moveit::core::RobotState& state)
+{
+  std::vector<double> joints;
+  for (const auto& name : jmg->getActiveJointModelNames())
+  {
+    joints.push_back(state.getVariablePosition(name));
+  }
+  return joints;
+}
+
+void TrajectoryFunctionsTestBase::attachToLink(moveit::core::RobotState& state, const moveit::core::LinkModel* link,
+                                               const std::string& object_name, const Eigen::Isometry3d& object_pose,
+                                               const moveit::core::FixedTransformsMap& subframes)
+{
+  state.attachBody(std::make_unique<moveit::core::AttachedBody>(
+      link, object_name, object_pose, std::vector<shapes::ShapeConstPtr>{}, EigenSTL::vector_Isometry3d{},
+      std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{}, subframes));
+}
+
 /**
  * @brief Parametrized class for tests with and without gripper.
  */
@@ -337,6 +403,119 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
   }
 }
 
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityCollisionObject)
+{
+  // Set up a default robot
+  moveit::core::RobotState state(robot_model_);
+  state.setToDefaultValues();
+  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+
+  std::vector<double> default_joints = getJoints(jmg, state);
+  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
+  Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity();
+  attachToLink(state, tip_link, "object", object_pose_in_tip, { {} });
+
+  // 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;
+  bool success = state.setFromIK(jmg, object_pose_in_base, "object");
+  EXPECT_TRUE(success);
+
+  // Given the target pose is the default pose of the object, the joints should be unchanged
+  std::vector<double> ik_joints = getJoints(jmg, state);
+  EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET));
+}
+
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedCollisionObject)
+{
+  // Set up a default robot
+  moveit::core::RobotState state(robot_model_);
+  state.setToDefaultValues();
+  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+
+  std::vector<double> default_joints = getJoints(jmg, state);
+  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 a non-trivial transform
+  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, { {} });
+
+  // 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;
+  bool success = state.setFromIK(jmg, object_pose_in_base, "object");
+  EXPECT_TRUE(success);
+
+  // Given the target pose is the default pose of the object, the joints should be unchanged
+  std::vector<double> ik_joints = getJoints(jmg, state);
+  EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET));
+}
+
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentitySubframe)
+{
+  // Set up a default robot
+  moveit::core::RobotState state(robot_model_);
+  state.setToDefaultValues();
+  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+
+  std::vector<double> default_joints = getJoints(jmg, state);
+  const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_);
+  Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_);
+
+  // Attach an object and subframe with no transforms
+  Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity();
+  Eigen::Isometry3d subframe_pose_in_object = Eigen::Isometry3d::Identity();
+  moveit::core::FixedTransformsMap subframes({ { "subframe", subframe_pose_in_object } });
+  attachToLink(state, tip_link, "object", object_pose_in_tip, subframes);
+
+  // The RobotState should be able to use a subframe pose to set the joints
+  Eigen::Isometry3d subframe_pose_in_base = tip_pose_in_base * object_pose_in_tip * subframe_pose_in_object;
+  bool success = state.setFromIK(jmg, subframe_pose_in_base, "object/subframe");
+  EXPECT_TRUE(success);
+
+  // Given the target pose is the default pose of the subframe, the joints should be unchanged
+  std::vector<double> ik_joints = getJoints(jmg, state);
+  EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET));
+}
+
+TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedSubframe)
+{
+  // Set up a default robot
+  moveit::core::RobotState state(robot_model_);
+  state.setToDefaultValues();
+  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
+
+  std::vector<double> default_joints = getJoints(jmg, state);
+  const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_);
+  Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_);
+
+  // Attach an object and subframe with non-trivial transforms
+  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());
+
+  Eigen::Isometry3d subframe_pose_in_object;
+  subframe_pose_in_object = Eigen::Translation3d(4, 5, 6);
+  subframe_pose_in_object *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitY());
+
+  moveit::core::FixedTransformsMap subframes({ { "subframe", subframe_pose_in_object } });
+  attachToLink(state, tip_link, "object", object_pose_in_tip, subframes);
+
+  // The RobotState should be able to use a subframe pose to set the joints
+  Eigen::Isometry3d subframe_pose_in_base = tip_pose_in_base * object_pose_in_tip * subframe_pose_in_object;
+  bool success = state.setFromIK(jmg, subframe_pose_in_base, "object/subframe");
+  EXPECT_TRUE(success);
+
+  // Given the target pose is the default pose of the subframe, the joints should be unchanged
+  std::vector<double> ik_joints = getJoints(jmg, state);
+  EXPECT_TRUE(jointsNear(ik_joints, default_joints, 4 * IK_SEED_OFFSET));
+}
+
 /**
  * @brief Test the wrapper function to compute inverse kinematics using robot
  * model