Update positioner support

twc_application/include/twc_application/raster_applicataion.h

@@ -36,6 +36,7 @@ class RasterApplication : public Application
 
     // Get global parameter
     use_rail_ = nh.param<bool>("/use_rail", use_rail_);
+    use_positioner_ = nh.param<bool>("/use_positioner", use_positioner_);
   }
 
   void run() override
@@ -49,19 +50,20 @@ class RasterApplication : public Application
     Eigen::Isometry3d post_transform = Eigen::Isometry3d::Identity() * Eigen::Translation3d(0, 0, 0.05) *
                                        Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()) *
                                        Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
-    Eigen::Isometry3d pre_transform = current_transforms["part_link"];
+    Eigen::Isometry3d pre_transform = Eigen::Isometry3d::Identity();
 
     tesseract_common::Toolpath paths = parsePathFromFile(toolpath_, pre_transform, post_transform);
     tesseract_common::Toolpath raster_strips = filterPath(paths);
 
     std::string motion_group;
-    if (use_rail_)
+    if (use_rail_ || use_positioner_)
       motion_group = "manipulator";
     else
       motion_group = "robot_only";
 
     tesseract_planning::ManipulatorInfo manip_info(motion_group);
     manip_info.tcp = tesseract_planning::ToolCenterPoint("st_tool0");
+    manip_info.working_frame = "part_link";
 
     tesseract_planning::CompositeInstruction program = createProgram(manip_info, raster_strips);
 
@@ -108,6 +110,13 @@ class RasterApplication : public Application
       freespace_profile = "FREESPACE_RAIL";
       joint_names = { "rail_joint_1", "robot_joint_1", "robot_joint_2", "robot_joint_3", "robot_joint_4", "robot_joint_5", "robot_joint_6" };
     }
+    else if (use_positioner_)
+    {
+      raster_profile = "RASTER_POSITIONER";
+      transition_profile = "TRANSITION_POSITIONER";
+      freespace_profile = "FREESPACE_POSITIONER";
+      joint_names = { "positioner_base_joint", "positioner_joint_1", "robot_joint_1", "robot_joint_2", "robot_joint_3", "robot_joint_4", "robot_joint_5", "robot_joint_6" };
+    }
 
     tesseract_planning::CompositeInstruction program("raster_program", tesseract_planning::CompositeInstructionOrder::ORDERED, manip_info);
 
@@ -173,6 +182,7 @@ class RasterApplication : public Application
 private:
   std::string toolpath_;
   bool use_rail_ {false};
+  bool use_positioner_ {false};
   actionlib::SimpleActionClient<tesseract_msgs::GetMotionPlanAction> ac_;
 };
 

twc_application/launch/application_positioner.launch

@@ -3,6 +3,7 @@
 
   <arg name="rviz" default="true" />
   <arg name="application" default="false" />
+  <arg name="use_positioner" default="false" />
 
   <!-- Tesseract Monitor Parameters -->
   <arg name="robot_description" default="robot_description"/>

twc_motion_planning/include/twc_motion_planning/utils.h

@@ -3,7 +3,7 @@
 
 #include <tesseract_motion_planners/robot_config.h>
 #include <tesseract_motion_planners/descartes/descartes_vertex_evaluator.h>
-#include <descartes_light/interface/edge_evaluator.h>
+#include <descartes_light/core/edge_evaluator.h>
 
 namespace twc
 {
@@ -16,10 +16,7 @@ enum class ProfileType
 
 inline bool isRobotConfigValid(tesseract_planning::RobotConfig config)
 {
-  if (config != tesseract_planning::RobotConfig::NUT && config != tesseract_planning::RobotConfig::FUT)
-    return false;
-
-  return true;
+  return (config == tesseract_planning::RobotConfig::NUT || config == tesseract_planning::RobotConfig::FUT);
 }
 
 inline bool isRobotConfigValid(tesseract_planning::RobotConfig start_config, tesseract_planning::RobotConfig end_config)
@@ -72,17 +69,17 @@ class RobotConfigEdgeEvaluator : public descartes_light::EdgeEvaluator<FloatType
 {
 public:
   RobotConfigEdgeEvaluator(tesseract_kinematics::ForwardKinematics::ConstPtr robot_only_kin)
-    : robot_only_kin_(robot_only_kin)
+    : robot_only_kin_(std::move(robot_only_kin))
   {
   }
 
-  std::pair<bool, FloatType> evaluate(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& start,
-                                      const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& end) const override
+  std::pair<bool, FloatType> evaluate(const descartes_light::State<FloatType>& start,
+                                      const descartes_light::State<FloatType>& end) const override
   {
     // Consider the edge:
     Eigen::Vector2i sign_correction(-1, 1);
-    auto start_config = tesseract_planning::getRobotConfig<FloatType>(robot_only_kin_, start.tail(robot_only_kin_->numJoints()), sign_correction);
-    auto end_config = tesseract_planning::getRobotConfig<FloatType>(robot_only_kin_, end.tail(robot_only_kin_->numJoints()), sign_correction);
+    auto start_config = tesseract_planning::getRobotConfig<FloatType>(robot_only_kin_, start.values.tail(robot_only_kin_->numJoints()), sign_correction);
+    auto end_config = tesseract_planning::getRobotConfig<FloatType>(robot_only_kin_, end.values.tail(robot_only_kin_->numJoints()), sign_correction);
 
     if (isRobotConfigValid(start_config, end_config))
       return std::make_pair(true, FloatType(0));
@@ -100,8 +97,8 @@ class WeightedEuclideanDistanceEdgeEvaluator : public descartes_light::EdgeEvalu
 public:
   WeightedEuclideanDistanceEdgeEvaluator(const Eigen::VectorXd& weights) : weights_(weights.cast<FloatType>()) {}
 
-  std::pair<bool, FloatType> evaluate(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& start,
-                                      const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& end) const override
+  std::pair<bool, FloatType> evaluate(const descartes_light::State<FloatType>& start,
+                                      const descartes_light::State<FloatType>& end) const override
   {
     FloatType cost = (weights_.array() * (end - start).array().abs()).sum();
     return std::make_pair(true, cost);

twc_motion_planning/src/twc_planning_server_node.cpp

@@ -36,7 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
 #include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>
 #include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
-#include <descartes_samplers/evaluators/compound_edge_evaluator.h>
+#include <descartes_light/edge_evaluators/compound_edge_evaluator.h>
 
 #include <tesseract_process_managers/taskflow_generators/descartes_taskflow.h>
 #include <tesseract_process_managers/taskflow_generators/trajopt_taskflow.h>

twc_support/config/workcell_positioner.srdf

@@ -7,9 +7,23 @@
         <chain base_link="ati_qc110f_base_link" tip_link="st_tool0"/>
     </group>
     <group name="positioner_only">
-        <chain base_link="positioner_base_link" tip_link="positioner_tool0"/>
+        <chain base_link="robot_base_link" tip_link="positioner_tool0"/>
     </group>
+
+    <group name="manipulator">
+        <chain base_link="positioner_tool0" tip_link="robot_tool0"/>
+    </group>
+
     <group_opw group="robot_only" a1="0.175" a2="-0.175" b="0" c1="0.495" c2="0.9" c3="0.96" c4="0.135" offsets="0.000000 0.000000 -1.570796 0.000000 0.000000 0.000000" sign_corrections="1 1 1 1 1 1"/>
+
+    <group_rep group="manipulator" solver_name="REPSolver1">
+      <manipulator group="robot_only" ik_solver="OPWInvKin" reach="2.3"/>
+      <positioner group="positioner_only">
+        <joint name="positioner_base_joint" resolution="0.1"/>
+        <joint name="positioner_joint_1" resolution="0.1"/>
+      </positioner>
+    </group_rep>
+
     <disable_collisions link1="positioner_base_link" link2="robot_link_2" reason="Never"/>
     <disable_collisions link1="positioner_base_link" link2="robot_base_link" reason="Never"/>
     <disable_collisions link1="ati_dock2_base_link" link2="robot_link_2" reason="Never"/>