Add support for external tcp attached to kinematic link

Author: Levi Armstrong

tesseract/tesseract_common/include/tesseract_common/manipulator_info.h

@@ -49,15 +49,22 @@ 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.
+   * @note External should also be set to true your kinematic object which includes say a robot and
+   * positioner, and 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_frame If empty assumed relative to world, otherwise the provided tcp is relative to this link.
+   * @note External should also be set to true your kinematic object which includes say a robot and
+   * positioner, and 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
@@ -80,15 +87,26 @@ 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.
+   * External should also be set to true your kinematic object which includes say a robot and
+   * positioner, and 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 verus the tool.
+   * @details External should also be set to true your kinematic object which includes say a robot and
+   * positioner, and 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_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp

@@ -119,14 +119,37 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
   auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
   if (it != active_links.end())
   {
-    if (mi.tcp.isExternal())
+    if (mi.tcp.isExternal() && mi.tcp.isString())
+    {
+      // If external, the part is attached to the robot so working frame is passed as link instead of target frame
+      // Since it is a link name then it has the possibility to also be dynamic so need to check
+      auto tcp_it = std::find(active_links.begin(), active_links.end(), mi.tcp.getString());
+      if (tcp_it != active_links.end())
+      {
+        // target_tcp, index, target, tcp relative to robot tip link, coeffs, robot tip link, term_type
+        ti = createDynamicCartesianWaypointTermInfo(Eigen::Isometry3d::Identity(),
+                                                    index,
+                                                    mi.tcp.getString(),
+                                                    cartesian_waypoint,
+                                                    cartesian_coeff,
+                                                    mi.working_frame,
+                                                    term_type);
+      }
+      else
+      {
+        ti = createCartesianWaypointTermInfo(
+            tcp, index, "", cartesian_waypoint, cartesian_coeff, mi.working_frame, term_type);
+      }
+    }
+    else if (mi.tcp.isExternal())
     {
       // If external, the part is attached to the robot so working frame is passed as link instead of target frame
       ti = createCartesianWaypointTermInfo(
-          tcp, index, "", cartesian_waypoint, cartesian_coeff, mi.working_frame, term_type);
+          tcp, index, mi.tcp.getExternalFrame(), cartesian_waypoint, cartesian_coeff, mi.working_frame, term_type);
     }
     else
     {
+      // target_tcp, index, target, tcp relative to robot tip link, coeffs, robot tip link, term_type
       ti = createDynamicCartesianWaypointTermInfo(
           cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
     }