Add external tool center point support

Author: Levi Armstrong

tesseract/tesseract/src/tesseract.cpp

@@ -384,7 +384,13 @@ Eigen::Isometry3d Tesseract::findTCP(const tesseract_planning::ManipulatorInfo&
     tesseract_environment::EnvState::ConstPtr env_state = environment_->getCurrentState();
     auto link_it = env_state->link_transforms.find(tcp_name);
     if (link_it != env_state->link_transforms.end())
-      return env_state->link_transforms.at(tip_link).inverse() * link_it->second;
+    {
+      // If it is external then the tcp is not attached to the robot
+      if (manip_info.tcp.isExternal())
+        return link_it->second;
+      else
+        return env_state->link_transforms.at(tip_link).inverse() * link_it->second;
+    }
 
     // Check callbacks for TCP
     for (const auto& fn : find_tcp_cb_)

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/manipulator_info.h

@@ -50,14 +50,16 @@ 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.
    */
-  ToolCenterPoint(const std::string& name);
+  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.
    */
-  ToolCenterPoint(const Eigen::Isometry3d& transform);
+  ToolCenterPoint(const Eigen::Isometry3d& transform, bool external = false);
 
   /**
    * @brief The Tool Center Point is empty
@@ -77,6 +79,19 @@ class ToolCenterPoint
    */
   bool isTransform() const;
 
+  /**
+   * @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.
+   * @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
+   */
+  void setExternal(bool value);
+
   /**
    * @brief Get the tool center point name
    * @return Name
@@ -95,13 +110,17 @@ class ToolCenterPoint
     ret_val &= (type_ == other.type_);
     ret_val &= (name_ == other.name_);
     ret_val &= (transform_.isApprox(transform_, 1e-5));
+    ret_val &= (external_ == other.external_);
     return ret_val;
   }
 
 protected:
   int type_{ 0 };
   std::string name_;
   Eigen::Isometry3d transform_;
+
+  /** @brief The external TCP is used when the robot is holding the part verus the tool. */
+  bool external_{ false };
 };
 
 /**

tesseract/tesseract_planning/tesseract_command_language/src/manipulator_info.cpp

@@ -36,13 +36,18 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-ToolCenterPoint::ToolCenterPoint(const std::string& name) : type_(1), name_(name) {}
+ToolCenterPoint::ToolCenterPoint(const std::string& name, bool external) : type_(1), name_(name), external_(external) {}
 
-ToolCenterPoint::ToolCenterPoint(const Eigen::Isometry3d& transform) : type_(2), transform_(transform) {}
+ToolCenterPoint::ToolCenterPoint(const Eigen::Isometry3d& transform, bool external)
+  : type_(2), transform_(transform), external_(external)
+{
+}
 
 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::getString() const
 {
@@ -179,6 +184,17 @@ ManipulatorInfo::ManipulatorInfo(const tinyxml2::XMLElement& xml_element)
 
       tcp = tcp_name;
     }
+
+    if (tcp_element->Attribute("external") != nullptr)
+    {
+      bool external = false;
+      tinyxml2::XMLError status = tcp_element->QueryBoolAttribute("external", &external);
+
+      if (status != tinyxml2::XML_SUCCESS)
+        throw std::runtime_error("ManipulatorInfo: Error parsing TCP attribute external.");
+
+      tcp.setExternal(external);
+    }
   }
 }
 
@@ -239,6 +255,7 @@ tinyxml2::XMLElement* ManipulatorInfo::toXML(tinyxml2::XMLDocument& doc) const
   {
     tinyxml2::XMLElement* xml_tcp = doc.NewElement("TCP");
     xml_tcp->SetAttribute("name", tcp.getString().c_str());
+    xml_tcp->SetAttribute("external", tcp.isExternal());
     xml_manip_info->InsertEndChild(xml_tcp);
   }
   else if (tcp.isTransform())
@@ -254,6 +271,7 @@ tinyxml2::XMLElement* ManipulatorInfo::toXML(tinyxml2::XMLDocument& doc) const
     wxyz_string << Eigen::Vector4d(q.w(), q.x(), q.y(), q.z()).format(eigen_format);
 
     xml_tcp->SetAttribute("wxyz", wxyz_string.str().c_str());
+    xml_tcp->SetAttribute("external", tcp.isExternal());
     xml_manip_info->InsertEndChild(xml_tcp);
   }
 

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

@@ -36,11 +36,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name,
-                                                         const PlannerRequest& request,
-                                                         const TrajOptPlanProfileMap& plan_profiles,
-                                                         const TrajOptCompositeProfileMap& composite_profiles,
-                                                         const TrajOptSolverProfileMap& solver_profiles);
+std::shared_ptr<trajopt::ProblemConstructionInfo>
+DefaultTrajoptProblemGenerator(const std::string& name,
+                               const PlannerRequest& request,
+                               const TrajOptPlanProfileMap& plan_profiles,
+                               const TrajOptCompositeProfileMap& composite_profiles,
+                               const TrajOptSolverProfileMap& solver_profiles);
 
 }  // namespace tesseract_planning
 #endif

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h

@@ -38,11 +38,12 @@ namespace tesseract_planning
 {
 class TrajOptMotionPlannerStatusCategory;
 
-using TrajOptProblemGeneratorFn = std::function<trajopt::TrajOptProb::Ptr(const std::string&,
-                                                                          const PlannerRequest&,
-                                                                          const TrajOptPlanProfileMap&,
-                                                                          const TrajOptCompositeProfileMap&,
-                                                                          const TrajOptSolverProfileMap&)>;
+using TrajOptProblemGeneratorFn =
+    std::function<std::shared_ptr<trajopt::ProblemConstructionInfo>(const std::string&,
+                                                                    const PlannerRequest&,
+                                                                    const TrajOptPlanProfileMap&,
+                                                                    const TrajOptCompositeProfileMap&,
+                                                                    const TrajOptSolverProfileMap&)>;
 
 class TrajOptMotionPlanner : public MotionPlanner
 {
@@ -81,9 +82,6 @@ class TrajOptMotionPlanner : public MotionPlanner
    */
   TrajOptPlanProfileMap plan_profiles;
 
-  /** @brief Optimization parameters to be used (Optional) */
-  sco::BasicTrustRegionSQPParameters params;
-
   /** @brief Callback functions called on each iteration of the optimization (Optional) */
   std::vector<sco::Optimizer::Callback> callbacks;
 

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp

@@ -34,11 +34,12 @@
 namespace tesseract_planning
 {
 /// @todo: Restructure this into several smaller functions that are testable and easier to understand
-trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name,
-                                                         const PlannerRequest& request,
-                                                         const TrajOptPlanProfileMap& plan_profiles,
-                                                         const TrajOptCompositeProfileMap& composite_profiles,
-                                                         const TrajOptSolverProfileMap& solver_profiles)
+std::shared_ptr<trajopt::ProblemConstructionInfo>
+DefaultTrajoptProblemGenerator(const std::string& name,
+                               const PlannerRequest& request,
+                               const TrajOptPlanProfileMap& plan_profiles,
+                               const TrajOptCompositeProfileMap& composite_profiles,
+                               const TrajOptSolverProfileMap& solver_profiles)
 {
   auto pci = std::make_shared<trajopt::ProblemConstructionInfo>(request.tesseract);
 
@@ -359,7 +360,6 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
   pci->basic_info.manip = composite_mi.manipulator;
   pci->basic_info.start_fixed = false;
   pci->basic_info.use_time = false;
-  //  pci->basic_info.convex_solver = optimizer;  // TODO: Fix this when port to trajopt_ifopt
 
   // Set trajopt seed
   assert(static_cast<long>(seed_states.size()) == pci->basic_info.n_steps);
@@ -376,10 +376,7 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
 
   cur_composite_profile->apply(*pci, 0, pci->basic_info.n_steps - 1, composite_mi, active_links, fixed_steps);
 
-  // Construct Problem
-  trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci);
-
-  return problem;
+  return pci;
 }
 
 }  // namespace tesseract_planning

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp

@@ -119,8 +119,17 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
   auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
   if (it != active_links.end())
   {
-    ti = createDynamicCartesianWaypointTermInfo(
-        cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
+    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);
+    }
+    else
+    {
+      ti = createDynamicCartesianWaypointTermInfo(
+          cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
+    }
   }
   else
   {

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_motion_planner.cpp

@@ -88,11 +88,7 @@ bool TrajOptMotionPlanner::terminate()
   return false;
 }
 
-void TrajOptMotionPlanner::clear()
-{
-  params = sco::BasicTrustRegionSQPParameters();
-  callbacks.clear();
-}
+void TrajOptMotionPlanner::clear() { callbacks.clear(); }
 
 MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared<TrajOptMotionPlanner>(); }
 
@@ -106,10 +102,11 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(const PlannerRequest& r
         tesseract_common::StatusCode(TrajOptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
     return response.status;
   }
-  trajopt::TrajOptProb::Ptr problem;
+
+  std::shared_ptr<trajopt::ProblemConstructionInfo> pci;
   if (request.data)
   {
-    problem = std::static_pointer_cast<trajopt::TrajOptProb>(request.data);
+    pci = std::static_pointer_cast<trajopt::ProblemConstructionInfo>(request.data);
   }
   else
   {
@@ -123,7 +120,7 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(const PlannerRequest& r
 
     try
     {
-      problem = problem_generator(name_, request, plan_profiles, composite_profiles, solver_profiles);
+      pci = problem_generator(name_, request, plan_profiles, composite_profiles, solver_profiles);
     }
     catch (std::exception& e)
     {
@@ -133,9 +130,12 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(const PlannerRequest& r
       return response.status;
     }
 
-    response.data = problem;
+    response.data = pci;
   }
 
+  // Construct Problem
+  trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci);
+
   // Set Log Level
   if (verbose)
     util::gLogLevel = util::LevelInfo;
@@ -144,7 +144,7 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(const PlannerRequest& r
 
   // Create optimizer
   sco::BasicTrustRegionSQP opt(problem);
-  opt.setParameters(params);
+  opt.setParameters(pci->opt_info);
   opt.initialize(trajToDblVec(problem->GetInitTraj()));
 
   // Add all callbacks