Add support for external tcp attached to kinematic link

tesseract/tesseract_common/include/tesseract_common/manipulator_info.h

@@ -48,16 +48,23 @@ class ToolCenterPoint
   /**
    * @brief Tool Center Point Defined by name
    * @param name The tool center point name
-   * @param external The external TCP is used when the robot is holding the part verus the tool.
+   * @param external The external TCP is used when the robot is holding the part versus the tool.
+   * @note External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
+   * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   ToolCenterPoint(const std::string& name, bool external = false);
 
   /**
    * @brief Tool Center Point Defined by transform
    * @param transform The tool center point transform
-   * @param external The external TCP is used when the robot is holding the part verus the tool.
+   * @param external The external TCP is used when the robot is holding the part versus the tool.
+   * @param external_frame If empty assumed relative to world, otherwise the provided tcp is relative to this link.
+   * @note External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
+   * anytime the tool is not attached to the tip link of the kinematic chain.
    */
-  ToolCenterPoint(const Eigen::Isometry3d& transform, bool external = false);
+  ToolCenterPoint(const Eigen::Isometry3d& transform, bool external = false, std::string external_frame = "");
 
   /**
    * @brief The Tool Center Point is empty
@@ -79,16 +86,27 @@ class ToolCenterPoint
 
   /**
    * @brief Check if tool center point is and external tcp which mean it is not defined
-   * @details The external TCP is used when the robot is holding the part verus the tool.
+   * @details The external TCP is used when the robot is holding the part versus the tool.
+   * External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
+   * anytime the tool is not attached to the tip link of the kinematic chain.
    * @return True if and external TCP, otherwise the false
    */
   bool isExternal() const;
 
   /**
    * @brief Set the external property
    * @param value True if external tool center point, otherwise false
+   * @param external_frame If an external tcp was defined as an Isometry then an external can be provided. If empty
+   * assumed relative to world.
    */
-  void setExternal(bool value);
+  void setExternal(bool value, std::string external_frame);
+
+  /**
+   * @brief If an external tcp was defined as an Isometry then an external frame can be provided.
+   * If empty assumed relative to world.
+   */
+  const std::string& getExternalFrame() const;
 
   /**
    * @brief Get the tool center point name
@@ -109,6 +127,7 @@ class ToolCenterPoint
     ret_val &= (name_ == other.name_);
     ret_val &= (transform_.isApprox(transform_, 1e-5));
     ret_val &= (external_ == other.external_);
+    ret_val &= (external_frame_ == other.external_frame_);
     return ret_val;
   }
 
@@ -117,8 +136,19 @@ class ToolCenterPoint
   std::string name_;
   Eigen::Isometry3d transform_;
 
-  /** @brief The external TCP is used when the robot is holding the part verus the tool. */
+  /**
+   * @brief The external TCP is used when the robot is holding the part versus the tool.
+   * @details External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
+   * anytime the tool is not attached to the tip link of the kinematic chain.
+   */
   bool external_{ false };
+
+  /**
+   * @brief If an external tcp was defined as an Isometry then an external can be provided. If empty assumed relative to
+   * world.
+   */
+  std::string external_frame_;
 };
 
 /**

tesseract/tesseract_common/src/manipulator_info.cpp

@@ -38,16 +38,22 @@ namespace tesseract_common
 {
 ToolCenterPoint::ToolCenterPoint(const std::string& name, bool external) : type_(1), name_(name), external_(external) {}
 
-ToolCenterPoint::ToolCenterPoint(const Eigen::Isometry3d& transform, bool external)
-  : type_(2), transform_(transform), external_(external)
+ToolCenterPoint::ToolCenterPoint(const Eigen::Isometry3d& transform, bool external, std::string external_frame)
+  : type_(2), transform_(transform), external_(external), external_frame_(std::move(external_frame))
 {
 }
 
 bool ToolCenterPoint::empty() const { return (type_ == 0); }
 bool ToolCenterPoint::isString() const { return (type_ == 1); }
 bool ToolCenterPoint::isTransform() const { return (type_ == 2); }
 bool ToolCenterPoint::isExternal() const { return external_; }
-void ToolCenterPoint::setExternal(bool value) { external_ = value; }
+
+const std::string& ToolCenterPoint::getExternalFrame() const { return external_frame_; }
+void ToolCenterPoint::setExternal(bool value, std::string external_frame)
+{
+  external_ = value;
+  external_frame_ = std::move(external_frame);
+}
 
 const std::string& ToolCenterPoint::getString() const
 {
@@ -193,7 +199,15 @@ ManipulatorInfo::ManipulatorInfo(const tinyxml2::XMLElement& xml_element)
       if (status != tinyxml2::XML_SUCCESS)
         throw std::runtime_error("ManipulatorInfo: Error parsing TCP attribute external.");
 
-      tcp.setExternal(external);
+      std::string external_frame;
+      if (tcp_element->Attribute("external_frame") != nullptr)
+      {
+        tinyxml2::XMLError status = QueryStringAttribute(tcp_element, "external_frame", external_frame);
+        if (status != tinyxml2::XML_SUCCESS)
+          throw std::runtime_error("ManipulatorInfo: Error parsing TCP attribute external_frame.");
+      }
+
+      tcp.setExternal(external, external_frame);
     }
   }
 }
@@ -272,6 +286,9 @@ tinyxml2::XMLElement* ManipulatorInfo::toXML(tinyxml2::XMLDocument& doc) const
 
     xml_tcp->SetAttribute("wxyz", wxyz_string.str().c_str());
     xml_tcp->SetAttribute("external", tcp.isExternal());
+    if (tcp.isExternal() && !tcp.getExternalFrame().empty())
+      xml_tcp->SetAttribute("external_frame", tcp.getExternalFrame().c_str());
+
     xml_manip_info->InsertEndChild(xml_tcp);
   }
 

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h

@@ -67,6 +67,34 @@ const std::vector<std::string>& getJointNames(const Waypoint& waypoint);
  */
 Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
 
+/**
+ * @brief Format the waypoints joint ordered by the provided joint names
+ *
+ * Throws if waypoint does not directly contain that information
+ *
+ * Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
+ * the program all match.
+ *
+ * @param joint_names The joint names defining the order desired
+ * @param waypoint The waypoint to format
+ * @return True if formating was required, otherwise false.
+ */
+bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint);
+
+/**
+ * @brief Check the waypoints joint order against the provided joint names
+ *
+ * Throws if waypoint does not directly contain that information
+ *
+ * Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
+ * the program all match.
+ *
+ * @param joint_names The joint names defining the order desired
+ * @param waypoint The waypoint to check format
+ * @return True if waypoint format is correct, otherwise false.
+ */
+bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
+
 /**
  * @brief Set the joint position for waypoints that contain that information
  * @param waypoint Waypoint to set

tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp

@@ -66,7 +66,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
   if (isJointWaypoint(waypoint))
   {
     const auto* jwp = waypoint.cast_const<JointWaypoint>();
-    jv = *jwp;
+    jv = jwp->waypoint;
     jn = jwp->joint_names;
   }
   else if (isStateWaypoint(waypoint))
@@ -80,7 +80,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
     throw std::runtime_error("Unsupported waypoint type.");
   }
 
-  if (jn.size() == joint_names.size())
+  if (jn.size() != joint_names.size())
     throw std::runtime_error("Joint name sizes do not match!");
 
   if (joint_names == jn)
@@ -103,6 +103,64 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
   return output;
 }
 
+bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint)
+{
+  Eigen::VectorXd* jv;
+  std::vector<std::string>* jn;
+  if (isJointWaypoint(waypoint))
+  {
+    auto* jwp = waypoint.cast<JointWaypoint>();
+    jv = &(jwp->waypoint);
+    jn = &(jwp->joint_names);
+  }
+  else if (isStateWaypoint(waypoint))
+  {
+    auto* swp = waypoint.cast<StateWaypoint>();
+    jv = &(swp->position);
+    jn = &(swp->joint_names);
+  }
+  else
+  {
+    throw std::runtime_error("Unsupported waypoint type.");
+  }
+
+  if (jn->size() != joint_names.size())
+    throw std::runtime_error("Joint name sizes do not match!");
+
+  if (joint_names == *jn)
+    return false;
+
+  Eigen::VectorXd output = *jv;
+  for (std::size_t i = 0; i < joint_names.size(); ++i)
+  {
+    if (joint_names[i] == (*jn)[i])
+      continue;
+
+    auto it = std::find(jn->begin(), jn->end(), joint_names[i]);
+    if (it == jn->end())
+      throw std::runtime_error("Joint names do not match!");
+
+    long idx = std::distance(jn->begin(), it);
+    output(static_cast<long>(i)) = (*jv)(static_cast<long>(idx));
+  }
+
+  *jn = joint_names;
+  *jv = output;
+
+  return true;
+}
+
+bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint)
+{
+  if (isJointWaypoint(waypoint))
+    return (joint_names == waypoint.cast_const<JointWaypoint>()->joint_names);
+
+  if (isStateWaypoint(waypoint))
+    return (joint_names == waypoint.cast_const<StateWaypoint>()->joint_names);
+
+  throw std::runtime_error("Unsupported waypoint type.");
+}
+
 bool setJointPosition(Waypoint& waypoint, const Eigen::Ref<const Eigen::VectorXd>& position)
 {
   if (isJointWaypoint(waypoint))

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h

@@ -183,6 +183,14 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
 CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_instructions,
                                        const tesseract_environment::Environment& env);
 
+/**
+ * @brief This formats the joint and state waypoints to align with the kinematics object
+ * @param composite_instructions The input program to format
+ * @param env The environment information
+ * @return True if the program required formating.
+ */
+bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env);
+
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_PLANNING_UTILS_H

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp

@@ -139,6 +139,10 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
   auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
   auto instructions_flattened = flattenProgram(request.instructions);
 
+  // Check inverse and forward kinematic objects
+  if (problem->manip_fwd_kin->getJointNames() != problem->manip_inv_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   // Loop over the flattened results and add them to response if the input was a plan instruction
   Eigen::Index dof = problem->manip_fwd_kin->numJoints();
   Eigen::Index result_index = 0;
@@ -157,7 +161,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
         auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
 
         Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++, dof);
-        move_instruction->getWaypoint().cast<StateWaypoint>()->position = temp.template cast<double>();
+        auto* swp = move_instruction->getWaypoint().cast<StateWaypoint>();
+        swp->position = temp.template cast<double>();
+        assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
       }
       else if (plan_instruction->isLinear())
       {
@@ -168,8 +174,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
         {
           Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++,
                                                                              dof);
-          instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
-              temp.template cast<double>();
+          auto* swp = instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
+          swp->position = temp.template cast<double>();
+          assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
         }
       }
       else if (plan_instruction->isFreespace())
@@ -190,8 +197,11 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
 
         assert(temp.cols() == static_cast<long>(move_instructions->size()) + 1);
         for (std::size_t i = 0; i < move_instructions->size(); ++i)
-          (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
-              temp.col(static_cast<long>(i) + 1);
+        {
+          auto* swp = (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
+          swp->position = temp.col(static_cast<long>(i) + 1);
+          assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
+        }
       }
       else
       {

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h

@@ -134,6 +134,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
   }
   else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
   {
+    assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
     const Eigen::VectorXd& position = getJointPosition(start_waypoint);
     cur_plan_profile->apply(*prob, position, *start_instruction, composite_mi, active_links, index);
   }
@@ -187,6 +188,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -214,6 +216,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
         }
         else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
           Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
           if (!prob->manip_fwd_kin->calcFwdKin(cur_pose, cur_position))
@@ -228,6 +231,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -262,6 +266,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
       {
         if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
 
           // Descartes does not support freespace so it will only include the plan instruction state, then in